575 lines
22 KiB
C#
575 lines
22 KiB
C#
using AgroBase.Services;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Threading.Tasks;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models.Modules
|
|
{
|
|
public class DirecionalModel
|
|
{
|
|
public string ID { get; set; } = T_Code.Dir.ToString();
|
|
|
|
// SETs
|
|
public byte _EnderecoCAN_Tx { get; set; }
|
|
public byte _EnderecoCAN_Rx { get; set; }
|
|
public string Mod_ID
|
|
{
|
|
get
|
|
{
|
|
return Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.DirMotor._EnderecoCAN_Tx == _EnderecoCAN_Tx)?.Modulo_ID ?? "";
|
|
}
|
|
}
|
|
public Sentido Sentido_SP { get; set; } = Sentido.Parado;
|
|
public double Angulo_SP { get; set; } = 0;
|
|
public double FolgaMecanica { get; set; } = 0;
|
|
public double Velocidade_Max { get; set; } = 600;
|
|
public int Velocidade
|
|
{
|
|
get
|
|
{
|
|
return Convert.ToInt32(((Variaveis.OperacaoEmAndamento.Parametros?.Controle?.DirVelocidadeMovimento ?? 0) / 100.0) * Velocidade_Max);
|
|
}
|
|
}
|
|
|
|
// GETs
|
|
public Sentido Sentido
|
|
{
|
|
get
|
|
{
|
|
return
|
|
AnguloLeitura > 0 ? Sentido.Horario :
|
|
AnguloLeitura < 0 ? Sentido.Antihorario :
|
|
Sentido.Parado;
|
|
}
|
|
}
|
|
public Sentido SentidoReal
|
|
{
|
|
get
|
|
{
|
|
if (Mod_ID == "EF" || Mod_ID == "DT")
|
|
{
|
|
return IN1_Atuado ? Sentido.Antihorario : Sentido.Horario;
|
|
}
|
|
else
|
|
{
|
|
return IN1_Atuado ? Sentido.Horario : Sentido.Antihorario;
|
|
}
|
|
}
|
|
}
|
|
public double AnguloDriver
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.AnguloPulsos ?? 0;
|
|
}
|
|
}
|
|
public double AnguloReal { get; set; } = double.NaN;
|
|
public bool UsarAnguloSensor { get; set; }
|
|
public double AnguloLeitura
|
|
{
|
|
get
|
|
{
|
|
if (!UsarAnguloSensor)
|
|
return AnguloDriver;
|
|
if (double.IsNaN(AnguloReal)) return AnguloDriver;
|
|
var sMAG = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Sensores?.FirstOrDefault(x => x.Componente == S_Code.sMAG && x.ID.Contains(Mod_ID));
|
|
if ((sMAG?.Inicializado ?? false) && (sMAG?.Aferir ?? false)) return AnguloReal;
|
|
return AnguloDriver;
|
|
}
|
|
}
|
|
public double AnguloFolgaCompensar { get; set; }
|
|
public double OffsetAnguloReal { get; set; }
|
|
public double RPM
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.RPM?.valor ?? 0;
|
|
}
|
|
}
|
|
public bool IN1_Atuado
|
|
{
|
|
get
|
|
{
|
|
return !(Leitura?.Entradas?[0] ?? false);
|
|
}
|
|
}
|
|
public bool IN2_Atuado
|
|
{
|
|
get
|
|
{
|
|
return !(Leitura?.Entradas?[1] ?? false);
|
|
}
|
|
}
|
|
public bool Referenciando { get; set; } = false;
|
|
public DateTime UltimoReferenciamento { get; set; }
|
|
public bool Referenciado { get; set; } = false;
|
|
|
|
public SensorModel SensorAnguloReal
|
|
{
|
|
get
|
|
{
|
|
var sMAG = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Sensores?.FirstOrDefault(x => x.Componente == S_Code.sMAG && x.ID.Contains(Mod_ID));
|
|
return sMAG;
|
|
}
|
|
}
|
|
|
|
public bool Comandar { get; set; } = false;
|
|
public bool Inicializado
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.Iniciado ?? false;
|
|
}
|
|
}
|
|
private MKS057DModel _leitura = null;
|
|
public MKS057DModel Leitura
|
|
{
|
|
get
|
|
{
|
|
_leitura = MKS057DCanService.DadosLeitura?.FirstOrDefault(x => x.EnderecoTx == _EnderecoCAN_Tx);
|
|
return _leitura;
|
|
}
|
|
set
|
|
{
|
|
_leitura = value;
|
|
}
|
|
}
|
|
|
|
public Direcao UltimaDirecao { get; set; } = Direcao.Parado;
|
|
public DateTime UltimoComandoEnviado { get; set; } = DateTime.MinValue;
|
|
public bool RetornandoAzero
|
|
{
|
|
get
|
|
{
|
|
bool foraZero = !FuncoesMatematicas.ValorEstaEntre(AnguloLeitura, 0, 1);
|
|
bool parar = UltimaDirecao == Direcao.Parado;
|
|
return foraZero && parar;
|
|
}
|
|
}
|
|
public bool EmMovimento
|
|
{
|
|
get
|
|
{
|
|
return RPM != 0;
|
|
}
|
|
}
|
|
public bool AtingiuAnguloSP
|
|
{
|
|
get
|
|
{
|
|
double anguloSp =
|
|
!Comandar || Sentido_SP == Sentido.Parado ? 0 :
|
|
Angulo_SP;
|
|
return FuncoesMatematicas.ValorEstaEntre(AnguloLeitura, anguloSp, 1);
|
|
}
|
|
}
|
|
|
|
public bool PosicaoTipoMovimentoConcluido(Direcao DirecaoAtual)
|
|
{
|
|
var TipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
|
|
switch (TipoMovimento)
|
|
{
|
|
case TipoMovimentoDirecional.RotacionarNoEixo:
|
|
case TipoMovimentoDirecional.MovimentoLateral:
|
|
int agSP = DirecaoAtual != Direcao.Parado ? VariaveisEquipamento.AnguloMovimento[TipoMovimento] : 0;
|
|
bool anguloCorreto = agSP == Convert.ToInt32(AnguloLeitura);
|
|
if (!anguloCorreto && agSP != 0)
|
|
{
|
|
if (agSP > 0)
|
|
{
|
|
anguloCorreto = AnguloLeitura >= agSP;
|
|
}
|
|
else
|
|
{
|
|
anguloCorreto = AnguloLeitura <= agSP;
|
|
}
|
|
}
|
|
return !Comandar || anguloCorreto;
|
|
default:
|
|
return !Comandar || Convert.ToInt32(Angulo_SP) == Convert.ToInt32(AnguloLeitura);
|
|
}
|
|
}
|
|
|
|
|
|
public Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor> ConfigSentidos { get; set; }
|
|
public List<FuncoesPinout> Funcoes { get; set; }
|
|
|
|
|
|
public static DirecionalModel CarregarParametrosIniciais(string Mod_ID, byte _EnderecoTx, byte _EnderecoRx)
|
|
{
|
|
return new DirecionalModel()
|
|
{
|
|
ID = T_Code.Dir.ToString(),
|
|
_EnderecoCAN_Tx = _EnderecoTx,
|
|
_EnderecoCAN_Rx = _EnderecoRx,
|
|
Comandar = true,
|
|
Funcoes = new List<FuncoesPinout>()
|
|
{
|
|
FuncoesPinout.PUL,
|
|
FuncoesPinout.DIR,
|
|
FuncoesPinout.ENA,
|
|
FuncoesPinout.EncoderAbsoluto
|
|
},
|
|
ConfigSentidos = new Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor>()
|
|
{
|
|
{
|
|
TipoMovimentoDirecional.RodasDianteiras,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Esquerda = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.RodasTraseiras,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Esquerda = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoDiagonal,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Horario : Sentido.Antihorario,
|
|
Esquerda = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Antihorario : Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoArco,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Esquerda = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.RotacionarNoEixo,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Horario,
|
|
Esquerda = Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoLateral,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Horario : Sentido.Antihorario,
|
|
Esquerda = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Antihorario : Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.Diagnostico,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Horario,
|
|
Esquerda = Sentido.Antihorario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
}
|
|
};
|
|
}
|
|
|
|
public void EnviarComandoControle(Direcao Direcao)
|
|
{
|
|
var op = Variaveis.OperacaoEmAndamento;
|
|
|
|
var _Simulando = op.Simulando;
|
|
|
|
if ((Inicializado && (Comandar || !AtingiuAnguloSP)) || _Simulando)
|
|
{
|
|
var _Controle = op.Controle;
|
|
int AnguloTipoMovimento = VariaveisEquipamento.AnguloMovimento[_Controle.TipoMovimento];
|
|
|
|
if (!Comandar)
|
|
{
|
|
Sentido_SP = Sentido.Parado;
|
|
}
|
|
else if (Direcao == Direcao.Direita)
|
|
{
|
|
Sentido_SP = ConfigSentidos[_Controle.TipoMovimento].Direita;
|
|
}
|
|
else if (Direcao == Direcao.Esquerda)
|
|
{
|
|
Sentido_SP = ConfigSentidos[_Controle.TipoMovimento].Esquerda;
|
|
}
|
|
else
|
|
{
|
|
Sentido_SP = Sentido.Parado;
|
|
}
|
|
|
|
UltimaDirecao = Direcao;
|
|
int mx = Sentido_SP == Sentido.Parado ? 0 : Sentido_SP == Sentido.Antihorario ? -1 : 1;
|
|
|
|
if (Sentido_SP == Sentido.Parado)
|
|
{
|
|
Angulo_SP = 0;
|
|
UltimaDirecao = Direcao.Parado;
|
|
}
|
|
else if (AnguloTipoMovimento != -1)
|
|
{
|
|
Angulo_SP = AnguloTipoMovimento;
|
|
}
|
|
else
|
|
{
|
|
Angulo_SP = _Controle.Angulo;
|
|
}
|
|
|
|
if (Comandar)
|
|
{
|
|
op.SimulacaoAnguloControle = op.Controle.Angulo;
|
|
//Console.WriteLine($"[{Mod_ID}] Angulo Atualizado para {Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle}");
|
|
}
|
|
|
|
Angulo_SP = Math.Abs(Angulo_SP) * mx;
|
|
Angulo_SP += AnguloFolgaCompensar;
|
|
|
|
if (_Simulando)
|
|
{
|
|
return;
|
|
}
|
|
|
|
//MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoMovimento(EnderecoByte, Angulo_SP, Velocidade, Sentido_SP));
|
|
MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, Angulo_SP, Velocidade, Sentido_SP);
|
|
UltimoComandoEnviado = DateTime.Now;
|
|
}
|
|
|
|
}
|
|
|
|
public async Task ReferenciaMotor()
|
|
{
|
|
var op = Variaveis.OperacaoEmAndamento;
|
|
|
|
if (Referenciando || !Inicializado) return;
|
|
|
|
Referenciando = true;
|
|
|
|
await Task.Delay(1000);
|
|
|
|
Referenciado = false;
|
|
|
|
bool sensorAngRealConectado = (SensorAnguloReal?.Inicializado ?? false);
|
|
|
|
if (sensorAngRealConectado)
|
|
{
|
|
Referenciado = await ReferenciaMotorAbsoluto();
|
|
}
|
|
else
|
|
{
|
|
Referenciado = await ReferenciaMotorRelativo();
|
|
}
|
|
|
|
if (Referenciado)
|
|
{
|
|
await SetZero();
|
|
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] - Referenciamento Concluído!");
|
|
op.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Operante, 100, $"Referenciamento Concluído!", id: Mod_ID);
|
|
}
|
|
else
|
|
{
|
|
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] - Referenciamento Falhou!");
|
|
op.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Falha, 0, $"Referenciamento Falhou!", id: Mod_ID);
|
|
}
|
|
|
|
Referenciando = false;
|
|
|
|
UltimoReferenciamento = DateTime.Now;
|
|
}
|
|
|
|
public async Task<bool> ReferenciaMotorAbsoluto()
|
|
{
|
|
double angControle = Math.Abs(AnguloReal);
|
|
|
|
if (angControle <= 0.8)
|
|
{
|
|
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorAbsoluto] [DIR {Mod_ID}] - RefAbs: AnguloReal já dentro da tolerância ({AnguloReal:F2}°).");
|
|
return true;
|
|
}
|
|
|
|
int velocidade = 10;
|
|
Sentido _sentido = AnguloReal > 0 ? Sentido.Antihorario : Sentido.Horario;
|
|
MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, angControle, velocidade, _sentido);
|
|
|
|
bool AtingiuSP() => AtingiuAnguloSP;
|
|
await FuncoesGlobais.AguardarCondicaoAsync(AtingiuSP, 5000, 200);
|
|
|
|
return AtingiuSP();
|
|
}
|
|
|
|
public async Task<bool> ReferenciaMotorRelativo()
|
|
{
|
|
await SetZero();
|
|
|
|
bool Sucesso = false;
|
|
bool EstadoInicial = IN1_Atuado;
|
|
double AnguloEntrada = 9999;
|
|
double AnguloSaida = 9999;
|
|
int verificacoesAngulo = 0;
|
|
double anguloAnterior = 0;
|
|
|
|
DateTime TimeoutGeral = DateTime.Now.AddMinutes(2); // Timeout geral para o processo.
|
|
|
|
Sentido _sentido = SentidoReal == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
|
double _anguloSP = 180 * (_sentido == Sentido.Antihorario ? -1 : 1);
|
|
double _anguloSpAnterior = 0;
|
|
|
|
while (!Sucesso && TimeoutGeral > DateTime.Now && Inicializado)
|
|
{
|
|
int VelocidadeGiro = DefineVelocidadeReferenciamento();
|
|
|
|
if (_anguloSP != _anguloSpAnterior || (verificacoesAngulo > 5 && AnguloLeitura == anguloAnterior))
|
|
{
|
|
MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, _anguloSP, VelocidadeGiro, _sentido);
|
|
_anguloSpAnterior = _anguloSP;
|
|
verificacoesAngulo = 0;
|
|
}
|
|
|
|
if (IN1_Atuado != EstadoInicial && AnguloEntrada == 9999)
|
|
{
|
|
AnguloEntrada = Leitura.AnguloEstimado();
|
|
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Angulo de Entrada Definido: {AnguloEntrada} (Leitura: {AnguloLeitura})");
|
|
|
|
_sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
|
_anguloSP *= -1;
|
|
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Invertendo Sentido de Giro para {_sentido.ToString()}");
|
|
}
|
|
if (IN1_Atuado == EstadoInicial && AnguloEntrada != 9999 && AnguloSaida == 9999)
|
|
{
|
|
AnguloSaida = Leitura.AnguloEstimado();
|
|
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Angulo de Saída Definido: {AnguloSaida} (Leitura: {AnguloLeitura})");
|
|
|
|
_sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
|
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Invertendo Sentido de Giro para {_sentido.ToString()}");
|
|
|
|
_anguloSP = CalcularCentroCompensado(AnguloEntrada, AnguloSaida, _sentido);
|
|
}
|
|
|
|
verificacoesAngulo++;
|
|
anguloAnterior = AnguloLeitura;
|
|
|
|
Sucesso = AnguloEntrada != 9999 && AnguloSaida != 9999 && (FuncoesMatematicas.ValorEstaEntre(Leitura.AnguloEstimado(), _anguloSP, 0.5));
|
|
|
|
DateTime Inicio = DateTime.Now;
|
|
bool NovoDado() => MKS057DCanService.DadosLeitura.Any(x => x.EnderecoRx == _EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio);
|
|
await FuncoesGlobais.AguardarCondicaoAsync(NovoDado, 500, 100);
|
|
}
|
|
|
|
if (!Sucesso)
|
|
{
|
|
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - ReferenciaMotorRelativo falhou (timeout ou não inicializado). Entrada={AnguloEntrada}, Saida={AnguloSaida}");
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Falha, 0, $"ReferenciaMotorRelativo falhou (timeout ou não inicializado). Entrada={AnguloEntrada}, Saida={AnguloSaida}", id: Mod_ID);
|
|
}
|
|
|
|
return Sucesso;
|
|
}
|
|
|
|
private int DefineVelocidadeReferenciamento()
|
|
{
|
|
int VelocidadeGiro = 10;
|
|
return VelocidadeGiro;
|
|
}
|
|
|
|
private double CalcularCentroCompensado(double entrada, double saida, Sentido sentido)
|
|
{
|
|
double centro = (entrada + saida) / 2.0;
|
|
double folga = Math.Abs(entrada - saida);
|
|
|
|
// Determina o sentido do giro baseado na diferença
|
|
double direcao = sentido == Sentido.Horario ? -1 : 1;
|
|
|
|
double compensacao = 0; // (folga / 2.0) * direcao;
|
|
|
|
double centroCompensado = centro + compensacao;
|
|
|
|
Variaveis.MostrarLog($"[DirecionalModel] [CalcularCentroCompensado] [DIR {Mod_ID}] - Folga: {folga}, Entrada: {entrada}, Saida: {saida}, Centro: {centro}, Compensacao: {compensacao}, Centro Compensado: {centroCompensado}");
|
|
|
|
FolgaMecanica = folga;
|
|
|
|
return centroCompensado;
|
|
}
|
|
|
|
public async Task SetZero()
|
|
{
|
|
MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx);
|
|
|
|
for (int i = 0; i < 5; i++)
|
|
{
|
|
DateTime Inicio = DateTime.Now;
|
|
bool NovoDado() => MKS057DCanService.DadosLeitura.Any(x => x.EnderecoRx == _EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio);
|
|
await FuncoesGlobais.AguardarCondicaoAsync(NovoDado, 500, 100);
|
|
|
|
if (FuncoesMatematicas.ValorEstaEntre(AnguloLeitura, -1, 1))
|
|
{
|
|
Variaveis.MostrarLog($"[DirecionalModel] [SetZero] [DIR {Mod_ID}] - Set Zero");
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx);
|
|
}
|
|
}
|
|
|
|
MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, 0, 10, Sentido.Parado);
|
|
Variaveis.MostrarLog($"[DirecionalModel] [SetZero] [DIR {Mod_ID}] - Comando Angulo Zero: {AnguloLeitura}");
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
public void RequisitarDadosSensoresExternos()
|
|
{
|
|
if (SensorAnguloReal?.Inicializado == true && SensorAnguloReal?.Aferir == true)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.DispSen.Dados.RequisitarDadosModulo(SensorAnguloReal.ID_Num, CanMessagePosicaoDados.Dados1, CanMessagePosicaoDados.Dados1, true);
|
|
}
|
|
}
|
|
|
|
public void AtualizarDadosSensoresExternos()
|
|
{
|
|
if (SensorAnguloReal != null)
|
|
{
|
|
double angulo = SensorAnguloReal.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.LeituraAngulo)?.atual?.valor ?? double.NaN;
|
|
if (!double.IsNaN(angulo))
|
|
{
|
|
double bruto = angulo + OffsetAnguloReal;
|
|
// normaliza pra faixa [-180, 180]
|
|
double norm = bruto % 360.0;
|
|
if (norm > 180.0) norm -= 360.0;
|
|
if (norm < -180.0) norm += 360.0;
|
|
|
|
AnguloReal = Math.Round(-norm, 2);
|
|
|
|
double folga = AnguloDriver - AnguloLeitura;
|
|
AnguloFolgaCompensar = AtingiuAnguloSP ? folga : 0;
|
|
}
|
|
else
|
|
{
|
|
AnguloReal = angulo;
|
|
AnguloFolgaCompensar = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|