From de5f1440f2073ae1ef06931a6ac6eb47ced09e54 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Mon, 23 Mar 2026 17:08:01 -0300 Subject: [PATCH] ajustes no controle --- .../AgroBase/Forms/IHM/frmDiagnosticos.cs | 4 +- AgroBase/AgroBase/Forms/IHM/frmDialogoMapa.cs | 17 +- .../Forms/Operacoes/frmResultadosOperacao.cs | 2 +- AgroBase/AgroBase/Forms/frmJoystick.cs | 17 +- AgroBase/AgroBase/Forms/frmOID.cs | 8 +- AgroBase/AgroBase/Forms/frmPrincipal.cs | 2 +- AgroBase/AgroBase/Models/GeneralJoystick.cs | 388 +++++++++++------- .../Models/Modules/MovimentacaoModel.cs | 2 +- .../Models/Operacoes/OperacaoModel.cs | 159 +++---- .../Models/TrajetoriaMapaOperacaoModel.cs | 7 +- AgroBase/AgroBase/Models/UdpCtrlMessage.cs | 24 +- AgroBase/AgroBase/Models/Variaveis.cs | 7 +- AgroBase/AgroBase/Services/LoRaSerializer.cs | 2 +- .../Operadores/ManagerWorkerService.cs | 2 + .../workers/manager_worker/modulos/mpc.py | 2 +- .../processadores/_4_em_andamento.py | 2 +- AgroBase/OperationControl/Models/Variaveis.cs | 24 +- 17 files changed, 396 insertions(+), 273 deletions(-) diff --git a/AgroBase/AgroBase/Forms/IHM/frmDiagnosticos.cs b/AgroBase/AgroBase/Forms/IHM/frmDiagnosticos.cs index 0b575ef4c..02b4a7851 100644 --- a/AgroBase/AgroBase/Forms/IHM/frmDiagnosticos.cs +++ b/AgroBase/AgroBase/Forms/IHM/frmDiagnosticos.cs @@ -799,7 +799,9 @@ namespace AgroBase.Forms.IHM Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.ForEach(x => x.MovMotor.Comandar = false); Modulo.MovMotor.Comandar = true; - GeneralJoystick.EnviaComandoMotor(Sentido == Direcao.Frente ? Keys.Up : Sentido == Direcao.Tras ? Keys.Down : Keys.Escape, T_Code.Mov); + //GeneralJoystick.EnviaComandoMotor(Sentido == Direcao.Frente ? Keys.Up : Sentido == Direcao.Tras ? Keys.Down : Keys.Escape, T_Code.Mov); + var b = Sentido == Direcao.Frente ? BotoesJoystick.Xis : Sentido == Direcao.Tras ? BotoesJoystick.Quadrado : BotoesJoystick.Vazio; + GeneralJoystick.ProcessarDadosControle(b, b == BotoesJoystick.Vazio, valorDesejado: Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent); DateTime Inicio = DateTime.Now; bool Sucesso = false; diff --git a/AgroBase/AgroBase/Forms/IHM/frmDialogoMapa.cs b/AgroBase/AgroBase/Forms/IHM/frmDialogoMapa.cs index 79a54806b..1fd5c914f 100644 --- a/AgroBase/AgroBase/Forms/IHM/frmDialogoMapa.cs +++ b/AgroBase/AgroBase/Forms/IHM/frmDialogoMapa.cs @@ -487,7 +487,9 @@ namespace AgroBase.Forms.IHM { // Lógica para enviar comando ao robô baseado em MovKey Console.WriteLine($"Movimento: MovKey={MovKey}"); - GeneralJoystick.EnviaComandoMotor(MovKey, Enums.T_Code.Mov); + //GeneralJoystick.EnviaComandoMotor(MovKey, Enums.T_Code.Mov); + var comando = GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == MovKey); + GeneralJoystick.ProcessarDadosControle(comando.botaoJoy, false, valorDesejado: Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent); prevMovKey = MovKey; // Atualiza o estado anterior } @@ -496,7 +498,9 @@ namespace AgroBase.Forms.IHM { // Lógica para enviar comando ao robô baseado em DirKey Console.WriteLine($"Direção: DirKey={DirKey}"); - GeneralJoystick.EnviaComandoMotor(DirKey, Enums.T_Code.Dir); + //GeneralJoystick.EnviaComandoMotor(DirKey, Enums.T_Code.Dir); + var comando = GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == DirKey); + GeneralJoystick.ProcessarDadosControle(comando.botaoJoy, false, valorDesejado: Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo); prevDirKey = DirKey; // Atualiza o estado anterior } } @@ -561,7 +565,8 @@ namespace AgroBase.Forms.IHM pnlFreio.Invalidate(); // Redesenha o painel // Enviar comando de freio - GeneralJoystick.EnviaComandoMotor(Keys.Space, Enums.T_Code.Mov); + //GeneralJoystick.EnviaComandoMotor(Keys.Space, Enums.T_Code.Mov); + GeneralJoystick.ProcessarDadosControle(BotoesJoystick.Bolinha, false); } } @@ -592,12 +597,14 @@ namespace AgroBase.Forms.IHM pnlFreio.Invalidate(); // Redesenha o painel // Enviar comando para desativar o freio - GeneralJoystick.EnviaComandoMotor(Keys.Escape, Enums.T_Code.Mov); + //GeneralJoystick.EnviaComandoMotor(Keys.Escape, Enums.T_Code.Mov); + GeneralJoystick.ProcessarDadosControle(BotoesJoystick.Bolinha, true); } else { // Freio travado, mantém travado - GeneralJoystick.EnviaComandoMotor(Keys.Space, Enums.T_Code.Mov); + //GeneralJoystick.EnviaComandoMotor(Keys.Space, Enums.T_Code.Mov); + GeneralJoystick.ProcessarDadosControle(BotoesJoystick.Bolinha, false); } } diff --git a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs index 5752ff7f9..84a07071f 100644 --- a/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs +++ b/AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs @@ -1157,7 +1157,7 @@ namespace AgroBase.Forms.Operacoes txtErvasTerreno.Text = Log.Atuador.PercentualErvasTerreno.ToString("0.00"); txtTipoMovimento.Text = Log.Controle.TipoMovimento.ToString(); txtVelocidade.Text = (Log.Movimentacao.VelocidadeMedia * 3.6).ToString("0.00") + " km/h (" + Log.Movimentacao.VelocidadeMedia.ToString("0.00") + " m/s)"; - txtDirecaoControle.Text = (Log.Controle?.Direcao ?? Enums.Direcao.Parado).ToString() + " (" + (Log.Controle?.Latencia ?? 0).ToString("0.00") + " s)"; + txtDirecaoControle.Text = (Log.Controle?.TiposControle?.FirstOrDefault(x => x.Tipo == Enums.T_Code.Dir)?.DirecaoAtual ?? Enums.Direcao.Parado).ToString() + " (" + (Log.Controle?.Latencia ?? 0).ToString("0.00") + " s)"; txtVelocidadeControle.Text = ((Log.Controle?.PercentualVelocidadeSP ?? 0) / VariaveisEquipamento.RPM_Max_Roda * 100).ToString() + " rpm"; txtErvasIdentificadas.Text = $"{totalAtuacoes}"; diff --git a/AgroBase/AgroBase/Forms/frmJoystick.cs b/AgroBase/AgroBase/Forms/frmJoystick.cs index dce6f88d7..1a098e572 100644 --- a/AgroBase/AgroBase/Forms/frmJoystick.cs +++ b/AgroBase/AgroBase/Forms/frmJoystick.cs @@ -1,4 +1,5 @@ -using System; +using AgroBase.Models; +using System; using System.Linq; using System.Windows.Forms; using static AgroBase.Models.Enums; @@ -27,13 +28,23 @@ namespace AgroBase.Forms private void frmJoystick_KeyDown(object sender, KeyEventArgs e) { var comando = GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == e.KeyCode); - GeneralJoystick.EnviaComandoMotor(e.KeyCode, comando?.Dispositivo ?? T_Code.Vzo); + if (comando == null) return; + switch (comando.Dispositivo) + { + case T_Code.Mov: + GeneralJoystick.ProcessarDadosControle(comando.botaoJoy, false, valorDesejado: Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent); + break; + case T_Code.Dir: + GeneralJoystick.ProcessarDadosControle(comando.botaoJoy, false, valorDesejado: Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo); + break; + } } private void frmJoystick_KeyUp(object sender, KeyEventArgs e) { var comando = GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == e.KeyCode); - GeneralJoystick.EnviaComandoMotor(Keys.Escape, comando?.Dispositivo ?? T_Code.Vzo); + if (comando == null) return; + GeneralJoystick.ProcessarDadosControle(comando.botaoJoy, true); } } diff --git a/AgroBase/AgroBase/Forms/frmOID.cs b/AgroBase/AgroBase/Forms/frmOID.cs index d92c41228..29de4cf2d 100644 --- a/AgroBase/AgroBase/Forms/frmOID.cs +++ b/AgroBase/AgroBase/Forms/frmOID.cs @@ -132,7 +132,7 @@ namespace AgroBase.Forms { //var Mod = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.MovMotor.EnderecoByte == Addr); //Mod.MovMotor.EnviarComandoControle(Enums.Direcao.Frente); - GeneralJoystick.EnviaComandoMotor(Keys.Up, Enums.T_Code.Mov); + GeneralJoystick.ProcessarDadosControle(Enums.BotoesJoystick.Xis, false, valorDesejado: Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent); } else { @@ -147,7 +147,7 @@ namespace AgroBase.Forms { //var Mod = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.MovMotor.EnderecoByte == Addr); //Mod.MovMotor.EnviarComandoControle(Enums.Direcao.Tras); - GeneralJoystick.EnviaComandoMotor(Keys.Down, Enums.T_Code.Mov); + GeneralJoystick.ProcessarDadosControle(Enums.BotoesJoystick.Quadrado, false, valorDesejado: Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent); } else { @@ -161,7 +161,7 @@ namespace AgroBase.Forms { //var Mod = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.MovMotor.EnderecoByte == Addr); //Mod.MovMotor.EnviarComandoControle(Enums.Direcao.Parado); - GeneralJoystick.EnviaComandoMotor(Keys.Escape, Enums.T_Code.Mov); + GeneralJoystick.ProcessarDadosControle(Enums.BotoesJoystick.Xis, true); } else { @@ -175,7 +175,7 @@ namespace AgroBase.Forms { //var Mod = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.MovMotor.EnderecoByte == Addr); //Mod.MovMotor.EnviarComandoControle(Enums.Direcao.EmFreio); - GeneralJoystick.EnviaComandoMotor(Keys.Space, Enums.T_Code.Mov); + GeneralJoystick.ProcessarDadosControle(Enums.BotoesJoystick.Bolinha, false); } else { diff --git a/AgroBase/AgroBase/Forms/frmPrincipal.cs b/AgroBase/AgroBase/Forms/frmPrincipal.cs index 45c04f0ae..7a8573f9c 100644 --- a/AgroBase/AgroBase/Forms/frmPrincipal.cs +++ b/AgroBase/AgroBase/Forms/frmPrincipal.cs @@ -182,7 +182,7 @@ namespace AgroBase trkKmh.Maximum = 100; trkRPM.Minimum = Variaveis.OperacaoEmAndamento.Parametros.Controle?.MovVelocidadeCErvasPercent ?? 15; - trkRPM.Maximum = VariaveisEquipamento.RPM_Max_Roda; + trkRPM.Maximum = Variaveis.OperacaoEmAndamento.Parametros.Controle?.MovVelocidadeSErvasPercent ?? 50; AtualizaRPM((int)Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP); diff --git a/AgroBase/AgroBase/Models/GeneralJoystick.cs b/AgroBase/AgroBase/Models/GeneralJoystick.cs index 12268908e..f95d92cb2 100644 --- a/AgroBase/AgroBase/Models/GeneralJoystick.cs +++ b/AgroBase/AgroBase/Models/GeneralJoystick.cs @@ -22,22 +22,18 @@ namespace AgroBase public static List Comandos = new List() { }; public static List DeParaComandos = new List() { - new DeParaComandos() { key = Keys.Left, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SEsquerda, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Left, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEEsquerda, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Right, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SDireita, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Right, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEDireita, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.R, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.R3, PararAoSoltar = false }, - new DeParaComandos() { key = Keys.Up, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Xis, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Up, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R2A, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Down, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Quadrado, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Down, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L2A, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Space, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Bolinha, PararAoSoltar = true }, - new DeParaComandos() { key = Keys.Subtract, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L1, PararAoSoltar = false }, - new DeParaComandos() { key = Keys.Add, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R1, PararAoSoltar = false }, - new DeParaComandos() { key = Keys.D1, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADEsquerda, PararAoSoltar = true, IDcontrolar = "B01" }, - new DeParaComandos() { key = Keys.D2, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADBaixo, PararAoSoltar = true, IDcontrolar = "B02" }, - new DeParaComandos() { key = Keys.D3, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADCima, PararAoSoltar = true, IDcontrolar = "B03" }, - new DeParaComandos() { key = Keys.D4, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADDireita, PararAoSoltar = true, IDcontrolar = "B04" }, + new DeParaComandos() { key = Keys.Left, botaoJoy = BotoesJoystick.SEsquerda, direcao = Direcao.Esquerda, Dispositivo = T_Code.Dir, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Left, botaoJoy = BotoesJoystick.AEEsquerda, direcao = Direcao.Esquerda, Dispositivo = T_Code.Dir, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Right, botaoJoy = BotoesJoystick.SDireita, direcao = Direcao.Direita, Dispositivo = T_Code.Dir, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Right, botaoJoy = BotoesJoystick.AEDireita, direcao = Direcao.Direita, Dispositivo = T_Code.Dir, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.R, botaoJoy = BotoesJoystick.R3, direcao = Direcao.Referenciar, Dispositivo = T_Code.Dir, PararAoSoltar = false }, + new DeParaComandos() { key = Keys.Up, botaoJoy = BotoesJoystick.Xis, direcao = Direcao.Frente, Dispositivo = T_Code.Mov, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Up, botaoJoy = BotoesJoystick.R2A, direcao = Direcao.Frente, Dispositivo = T_Code.Mov, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Down, botaoJoy = BotoesJoystick.Quadrado, direcao = Direcao.Tras, Dispositivo = T_Code.Mov, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Down, botaoJoy = BotoesJoystick.L2A, direcao = Direcao.Tras, Dispositivo = T_Code.Mov, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Space, botaoJoy = BotoesJoystick.Bolinha, direcao = Direcao.EmFreio, Dispositivo = T_Code.Mov, PararAoSoltar = true }, + new DeParaComandos() { key = Keys.Subtract, botaoJoy = BotoesJoystick.L1, Dispositivo = T_Code.Mov, PararAoSoltar = false }, + new DeParaComandos() { key = Keys.Add, botaoJoy = BotoesJoystick.R1, Dispositivo = T_Code.Mov, PararAoSoltar = false }, }; public static List BotoesJoyPressionados = new List() { }; public static List MovimentosCombinados = new List() @@ -185,8 +181,10 @@ namespace AgroBase } catch { - EnviaComandoMotor(Keys.Escape, T_Code.Mov, ForcarComando: true); - EnviaComandoMotor(Keys.Escape, T_Code.Dir, ForcarComando: true); + //EnviaComandoMotor(Keys.Escape, T_Code.Mov, ForcarComando: true); + //EnviaComandoMotor(Keys.Escape, T_Code.Dir, ForcarComando: true); + ProcessarDadosControle(BotoesJoystick.Xis, true); + ProcessarDadosControle(BotoesJoystick.SEsquerda, true); AtualizaDispositivo(); return; } @@ -517,127 +515,73 @@ namespace AgroBase .ToList(); botoes_pressionados_g.ForEach(botao => { - EnviarComandoMotorAnalogico(botao.botao, botao.dados?.LastOrDefault()?.valor ?? 0, false); + ProcessarDadosControle(botao.botao, false, valorJoy: botao.dados?.LastOrDefault()?.valor ?? 0); //Console.WriteLine($"Botao pressionado: {botao.botao}, Valor: {string.Join(", ", botao.dados.Select(y => y.valor))}"); BotoesJoyPressionados.Add(botao.botao); - var Comando = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao.botao); - if (Comando != null) - { - EnviaComandoMotor(Comando.key, Comando.Dispositivo, ID: Comando.IDcontrolar, botao: botao.botao); - } }); _botoesSoltos.ForEach(botao => { - EnviarComandoMotorAnalogico(botao, 0, true); + ProcessarDadosControle(botao, true, valorJoy: 0); //Console.WriteLine($"Botao solto: {botao}"); BotoesJoyPressionados.Remove(botao); - var Comando = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao); - if (Comando != null && Comando.PararAoSoltar) - { - EnviaComandoMotor(Keys.Escape, Comando.Dispositivo, ID: Comando.IDcontrolar, botao: botao, ForcarComando: true); - } }); } - private static void EnviarComandoMotorAnalogico(BotoesJoystick botao, double valor, bool solto) + public static void ProcessarDadosControle(BotoesJoystick botao, bool solto, double? valorJoy = null, double? valorDesejado = null, string ID = "", bool ForcarComando = false) { - T_Code dispositivo = T_Code.Vzo; - Direcao direcao = Direcao.Parado; - double percent = 0; - double centro = 32767; - double deadzone = 3000; - double maximo = 65535.0; - var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle; + var comando = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao)?.Clone(); + comando?.CalcularValor(solto, valorJoy: valorJoy, valorDesejado: valorDesejado); + if (comando != null) + EnviarComandoMotorAtualizado(comando, ID: ID, ForcarComando: ForcarComando); + } - switch (botao) - { - case BotoesJoystick.AEEsquerda: - dispositivo = T_Code.Dir; - direcao = Direcao.Esquerda; - if (valor < centro - deadzone) - percent = (centro - valor) / centro; - else - percent = 0; - break; - - case BotoesJoystick.AEDireita: - direcao = Direcao.Direita; - dispositivo = T_Code.Dir; - if (valor > centro + deadzone) - percent = (valor - centro) / (maximo - centro); - else - percent = 0; - break; - - case BotoesJoystick.AECima: - if (valor < centro - deadzone) - percent = (centro - valor) / centro; - else - percent = 0; - break; - - case BotoesJoystick.AEBaixo: - if (valor > centro + deadzone) - percent = (valor - centro) / (maximo - centro); - else - percent = 0; - break; - - case BotoesJoystick.SEsquerda: - direcao = Direcao.Esquerda; - dispositivo = T_Code.Dir; - percent = 1.0; - break; - - case BotoesJoystick.SDireita: - direcao = Direcao.Direita; - dispositivo = T_Code.Dir; - percent = 1.0; - break; - - case BotoesJoystick.L2A: - direcao = Direcao.Tras; - dispositivo = T_Code.Mov; - percent = valor / maximo; - break; - - case BotoesJoystick.R2A: - direcao = Direcao.Frente; - dispositivo = T_Code.Mov; - percent = valor / maximo; - break; - - case BotoesJoystick.Xis: - direcao = Direcao.Frente; - dispositivo = T_Code.Mov; - percent = ((double)pControle.MovVelocidadeCErvasPercent / (double)pControle.MovVelocidadeSErvasPercent); - break; - - case BotoesJoystick.Quadrado: - direcao = Direcao.Tras; - dispositivo = T_Code.Mov; - percent = ((double)pControle.MovVelocidadeCErvasPercent / (double)pControle.MovVelocidadeSErvasPercent); - break; - - } - - percent = FuncoesMatematicas.Clamp(percent, 0, 1); - - //Console.WriteLine($"botao: {botao}, valor: {valor}, percentual: {percent * 100.0:F2}%"); + private static void EnviarComandoMotorAtualizado(DeParaComandos Comando, string ID = "", bool ForcarComando = false) + { + bool deveInterromper = + !Forms.IHM.frmDiagnosticos.Diagnosticando && !ForcarComando && + ( + Variaveis.OperacaoEmAndamento.Modo == ModoOperacao.Manual && ( + Variaveis.OperacaoEmAndamento.Calibrando || + !Variaveis.OperacaoEmAndamento.OperacaoLiberada || + !Variaveis.OperacaoEmAndamento.Iniciado || + !VerificaComandoValidoSonar(Comando.key) + ) + ); + if (deveInterromper) return; var _Controle = Variaveis.OperacaoEmAndamento.Controle; - _Controle.Direcao = direcao; - int mx = direcao == Direcao.Esquerda || direcao == Direcao.Tras ? -1 : 1; - switch (dispositivo) + if (Comando.direcao.HasValue) + _Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo).DirecaoAtual = Comando.direcao.Value; + + for (int i = 0; i < 1; i++) { - case T_Code.Dir: - _Controle.Angulo = solto ? 0 : pControle.DirAnguloMaximo * percent * mx; - break; - case T_Code.Mov: - double map = FuncoesMatematicas.Map(valor, 0, maximo, pControle.MovVelocidadeCErvasPercent, pControle.MovVelocidadeSErvasPercent); - _Controle.PercentualVelocidadeSP = solto ? 0 : map * mx; - break; + switch (Comando.Dispositivo) + { + case T_Code.Mov: + _Controle.PercentualVelocidadeSP = Comando.ValorAplicar; + _Controle.EmFreio = Comando.EstadoAplicar; + Variaveis.OperacaoEmAndamento.AtualizarDadosControle(ForcarComando, new List() { Comando.Dispositivo }); + break; + case T_Code.Dir: + if (MovimentosCombinados.Contains(Variaveis.OperacaoEmAndamento.Controle.TipoMovimento)) + { + Task.Run(async () => await Variaveis.OperacaoEmAndamento.DispMvd.Dados.ExecutarMovimentoDirecionalCombinado(Comando.direcao.Value)); + } + else if (Comando.direcao == Direcao.Referenciar) + { + List modsControlar = (!string.IsNullOrEmpty(ID)) ? new List() { ID } : null; + Task.Run(async () => await Variaveis.OperacaoEmAndamento.DispMvd.Dados.RealizarReferenciamentoDirecional(ModsReferenciar: modsControlar)); + } + else + { + _Controle.Angulo = Comando.ValorAplicar; + Variaveis.OperacaoEmAndamento.AtualizarDadosControle(ForcarComando, new List() { Comando.Dispositivo }); + } + break; + } } + + AtualizaConsole(_Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo).DirecaoAtual.ToString()); } public static void EnviaComandoMotor(Keys Tecla, T_Code Disp = T_Code.Vzo, string ID = null, BotoesJoystick botao = BotoesJoystick.Vazio, bool ForcarComando = false) @@ -775,25 +719,6 @@ namespace AgroBase { Comandos.Add(Comando); - /*for (int i = 0; i < 1; i++) - { - var Dispositivo = DispCon.FirstOrDefault(x => x.Dispositivo == Comando.Dispositivo); - if (Dispositivo != null) - { - var Protocolos = - Dispositivo.Dados is MovimentacaoModel ? ((MovimentacaoModel)Dispositivo.Dados).Motores.Select(x => x.ProtocoloComando(DirecaoAtual)) : - Dispositivo.Dados is DirecionalModel ? ((DirecionalModel)Dispositivo.Dados).Motores.Select(x => x.ProtocoloComando(DirecaoAtual)) : - null; - foreach (var ProtocoloMotor in Protocolos) - { - Dispositivo.EnviarDadosSerial(ProtocoloMotor); - Task.Delay(10); - } - } - - Task.Delay(100); - }*/ - for (int i = 0; i < 1; i++) { switch (Comando.Dispositivo) @@ -986,18 +911,6 @@ namespace AgroBase if (DeveParar) { ComandosImpedidos.Add(Keys.Up); - /*switch (Sonar.Resumo.DirecaoDesvio) - { - case Direcao.Parado: - ComandosImpedidos.Add(Keys.Up); - break; - case Direcao.Esquerda: - ComandosImpedidos.Add(Keys.Right); - break; - case Direcao.Direita: - ComandosImpedidos.Add(Keys.Left); - break; - }*/ } if (UltrasonicA05Service.Iniciado) @@ -1041,6 +954,171 @@ namespace AgroBase public T_Code Dispositivo { get; set; } public bool PararAoSoltar { get; set; } public string IDcontrolar { get; set; } + public Direcao? direcao { get; set; } + public double ValorAplicar { get; set; } + public bool EstadoAplicar { get; set; } + + public void CalcularValor(bool solto, double? valorJoy = null, double? valorDesejado = null) + { + double centro = 32767.0; + double deadzone = 3000.0; + + var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle; + var controle = Variaveis.OperacaoEmAndamento.Controle; + + double velMin = pControle.MovVelocidadeCErvasPercent; + double velMax = pControle.MovVelocidadeSErvasPercent; + + double valorSaida = 0; + bool estadoSaida = false; + + switch (botaoJoy) + { + // ========================================================= + // DIREÇÃO ANALÓGICA + // ========================================================= + case BotoesJoystick.AEEsquerda: + { + double percent = 0; + if (valorJoy != null) + { + if (!solto && valorJoy.Value < centro - deadzone) + percent = (centro - valorJoy.Value) / centro; + percent = FuncoesMatematicas.Clamp(percent, 0, 1); + valorSaida = solto ? 0 : pControle.DirAnguloMaximo * percent; + } + else if (valorDesejado != null) + { + valorSaida = solto ? 0 : valorDesejado.Value; + } + break; + } + + case BotoesJoystick.AEDireita: + { + if (valorJoy != null) + { + double percent = 0; + if (!solto && valorJoy.Value > centro + deadzone) + percent = (valorJoy.Value - centro) / (GeneralJoystick.MaximoAnalogico - centro); + percent = FuncoesMatematicas.Clamp(percent, 0, 1); + valorSaida = solto ? 0 : pControle.DirAnguloMaximo * percent; + } + else if (valorDesejado != null) + { + valorSaida = valorDesejado.Value; + } + break; + } + + // Não usados por enquanto + case BotoesJoystick.AECima: + case BotoesJoystick.AEBaixo: + break; + + // ========================================================= + // DIREÇÃO DIGITAL + // ========================================================= + case BotoesJoystick.SEsquerda: + case BotoesJoystick.SDireita: + valorSaida = solto ? 0 : valorDesejado ?? pControle.DirAnguloMaximo; + break; + + // ========================================================= + // MOVIMENTO ANALÓGICO + // ========================================================= + case BotoesJoystick.L2A: + case BotoesJoystick.R2A: + { + if (solto) + { + valorSaida = 0; + } + else if (valorJoy != null) + { + double gatilho = FuncoesMatematicas.Clamp(valorJoy.Value / GeneralJoystick.MaximoAnalogico, 0, 1); + valorSaida = FuncoesMatematicas.Map(gatilho, 0, 1, velMin, velMax); + } + else if (valorDesejado != null) + { + valorSaida = valorDesejado.Value; + } + break; + } + + // ========================================================= + // MOVIMENTO DIGITAL FIXO NO MÍNIMO + // ========================================================= + case BotoesJoystick.Xis: + case BotoesJoystick.Quadrado: + valorSaida = solto ? 0 : valorDesejado ?? velMin; + break; + + // ========================================================= + // FREIO + // ========================================================= + case BotoesJoystick.Bolinha: + valorSaida = 0; + estadoSaida = !solto; + break; + + // ========================================================= + // AJUSTE FINO DE VELOCIDADE + // Só funciona se já estiver em movimento + // ========================================================= + case BotoesJoystick.L1: + { + if (solto) + { + valorSaida = controle.PercentualVelocidadeSP; + } + else + { + var ultima_direcao = controle.TiposControle?.FirstOrDefault(x => x.Tipo == Dispositivo)?.UltimaDirecao ?? Direcao.Parado; + bool emMovimento = ultima_direcao == Direcao.Frente || ultima_direcao == Direcao.Tras; + if (emMovimento && controle.PercentualVelocidadeSP > 0) + valorSaida = Math.Max(velMin, controle.PercentualVelocidadeSP - 1); + } + break; + } + + case BotoesJoystick.R1: + { + if (solto) + { + valorSaida = controle.PercentualVelocidadeSP; + } + else + { + var ultima_direcao = controle.TiposControle?.FirstOrDefault(x => x.Tipo == T_Code.Mov)?.UltimaDirecao ?? Direcao.Parado; + bool emMovimento = ultima_direcao == Direcao.Frente || ultima_direcao == Direcao.Tras; + if (emMovimento && controle.PercentualVelocidadeSP > 0) + valorSaida = Math.Min(velMax, controle.PercentualVelocidadeSP + 1); + } + break; + } + } + + if (solto && PararAoSoltar) + direcao = Direcao.Parado; + ValorAplicar = valorSaida; + EstadoAplicar = estadoSaida; + } + + public DeParaComandos Clone() + { + return new DeParaComandos() + { + botaoJoy = botaoJoy, + key = key, + Dispositivo = Dispositivo, + PararAoSoltar = PararAoSoltar, + IDcontrolar = IDcontrolar, + direcao = direcao, + ValorAplicar = ValorAplicar, + EstadoAplicar = EstadoAplicar, + }; + } } public enum ComparativoAnalogico @@ -1056,4 +1134,6 @@ namespace AgroBase public int valor { get; set; } } + + } diff --git a/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs b/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs index f4c8e400c..c60a28be3 100644 --- a/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs +++ b/AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs @@ -394,7 +394,7 @@ namespace AgroBase.Models.Modules 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; + RPM_SP = Math.Abs(RpmCorrigido); FatorCompensacaoCurvasAnterior = FatorCompensacaoCurvas; Variaveis.OperacaoEmAndamento.SimulacaoRpmControle = _Controle.PercentualVelocidadeSP; diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index f2c5ffbb0..17c47a617 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -441,10 +441,9 @@ namespace AgroBase.Models Variaveis.OperacaoEmAndamento.Controle = new OperacaoControleModel() { TipoMovimento = TipoMovimentoDirecional.RodasDianteiras, - Angulo = 25, - Direcao = Direcao.Parado, + Angulo = 0, EmFreio = false, - PercentualVelocidadeSP = 20, + PercentualVelocidadeSP = 0, AlturaBarra = 50, TiposControle = new List() { @@ -455,7 +454,9 @@ namespace AgroBase.Models DelayEnvioComando = 250, DelayEnvioComandoExtra = 0, Comandos = new List(), - UltimoComando = DateTime.Now + UltimoComando = DateTime.Now, + DirecaoAtual = Direcao.Parado, + UltimaDirecao = Direcao.Parado }, new OperacaoControleTipoModel() { @@ -464,7 +465,9 @@ namespace AgroBase.Models DelayEnvioComando = 250, DelayEnvioComandoExtra = 0, Comandos = new List(), - UltimoComando = DateTime.Now + UltimoComando = DateTime.Now, + DirecaoAtual = Direcao.Parado, + UltimaDirecao = Direcao.Parado }, new OperacaoControleTipoModel() { @@ -577,7 +580,6 @@ namespace AgroBase.Models { TipoMovimento = TipoMovimentoDirecional.RodasDianteiras, Angulo = 0, - Direcao = Direcao.Parado, EmFreio = false, PercentualVelocidadeSP = 0, AlturaBarra = 50, @@ -590,7 +592,9 @@ namespace AgroBase.Models DelayEnvioComando = 500, DelayEnvioComandoExtra = 0, Comandos = new List(), - UltimoComando = DateTime.Now + UltimoComando = DateTime.Now, + DirecaoAtual = Direcao.Parado, + UltimaDirecao = Direcao.Parado }, new OperacaoControleTipoModel() { @@ -599,7 +603,9 @@ namespace AgroBase.Models DelayEnvioComando = 300, DelayEnvioComandoExtra = 0, Comandos = new List(), - UltimoComando = DateTime.Now + UltimoComando = DateTime.Now, + DirecaoAtual = Direcao.Parado, + UltimaDirecao = Direcao.Parado }, new OperacaoControleTipoModel() { @@ -711,7 +717,6 @@ namespace AgroBase.Models { TipoMovimento = TipoMovimentoDirecional.RodasDianteiras, Angulo = 0, - Direcao = Direcao.Parado, EmFreio = false, PercentualVelocidadeSP = 0, TiposControle = new List() @@ -723,7 +728,9 @@ namespace AgroBase.Models DelayEnvioComando = 500, DelayEnvioComandoExtra = 0, Comandos = new List(), - UltimoComando = DateTime.Now + UltimoComando = DateTime.Now, + DirecaoAtual = Direcao.Parado, + UltimaDirecao = Direcao.Parado }, new OperacaoControleTipoModel() { @@ -732,7 +739,9 @@ namespace AgroBase.Models DelayEnvioComando = 1000, DelayEnvioComandoExtra = 0, Comandos = new List(), - UltimoComando = DateTime.Now + UltimoComando = DateTime.Now, + DirecaoAtual = Direcao.Parado, + UltimaDirecao = Direcao.Parado }, new OperacaoControleTipoModel() { @@ -814,7 +823,6 @@ namespace AgroBase.Models { TipoMovimento = TipoMovimentoDirecional.RodasDianteiras, Angulo = 0, - Direcao = Direcao.Parado, EmFreio = false, PercentualVelocidadeSP = 0, TiposControle = new List() @@ -826,7 +834,9 @@ namespace AgroBase.Models DelayEnvioComando = 500, DelayEnvioComandoExtra = 0, Comandos = new List(), - UltimoComando = DateTime.Now + UltimoComando = DateTime.Now, + DirecaoAtual = Direcao.Parado, + UltimaDirecao = Direcao.Parado }, new OperacaoControleTipoModel() { @@ -835,7 +845,9 @@ namespace AgroBase.Models DelayEnvioComando = 200, DelayEnvioComandoExtra = 0, Comandos = new List(), - UltimoComando = DateTime.Now + UltimoComando = DateTime.Now, + DirecaoAtual = Direcao.Parado, + UltimaDirecao = Direcao.Parado } }, }; @@ -1344,85 +1356,55 @@ namespace AgroBase.Models var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle; var _Controle = Variaveis.OperacaoEmAndamento.Controle; var _ControleAnterior = Variaveis.OperacaoEmAndamento.ControleAnterior; + var DirecaoAtual = _Controle.TiposControle.FirstOrDefault(x => x.Tipo == Tipo).DirecaoAtual; switch (Tipo) { case T_Code.Mov: { - if (_Controle.EmFreio) - { - _Controle.Direcao = Direcao.EmFreio; - } - else if (_Controle.RPM_SP == 0) - { - _Controle.Direcao = Direcao.Parado; - } - else if (_Controle.RPM_SP < 0) - { - _Controle.PercentualVelocidadeSP *= -1; - _Controle.Direcao = Direcao.Tras; - } - else if (_Controle.RPM_SP > 0) - { - _Controle.Direcao = Direcao.Frente; - } - var modsAtualizar = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.Where(Mod => Mod.MovMotor.NovoRPMNecessario).Select(x => x.Modulo_ID).ToList(); bool envioNecessario = ForcarEnvio || _Controle.RPM_SP != _ControleAnterior.RPM_SP || _Controle.EmFreio != _ControleAnterior.EmFreio; if (envioNecessario || modsAtualizar.Any()) { foreach (var Mod in Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.Where(x => envioNecessario ? true : modsAtualizar.Contains(x.Modulo_ID))) { - Mod.MovMotor.EnviarComandoControle(_Controle.Direcao); + Mod.MovMotor.EnviarComandoControle(DirecaoAtual); } - - _Controle.TiposControle.FirstOrDefault(x => x.Tipo == Tipo).UltimaDirecao = _Controle.Direcao; } + //Variaveis.MostrarLog($"Velocidade SP alterada de {_ControleAnterior.PercentualVelocidadeSP} para {_Controle.PercentualVelocidadeSP}"); + _ControleAnterior.PercentualVelocidadeSP = _Controle.PercentualVelocidadeSP; - _ControleAnterior.Direcao = _Controle.Direcao; _ControleAnterior.EmFreio = _Controle.EmFreio; RedisService.AtualizarCampos( CtxKey.DadosControle, - ("velocidade_sp_ant", _Controle.PercentualVelocidadeSP) + ("velocidade_sp_ant", _Controle.PercentualVelocidadeSP), + ("em_freio_ant", _Controle.EmFreio) ); Protocolos = new List() { - _Controle.Direcao.ToString() + ";" + _Controle.RPM_SP.ToString() + ";" + (ForcarEnvio ? "1": "0") + DirecaoAtual.ToString() + ";" + _Controle.RPM_SP.ToString() + ";" + (ForcarEnvio ? "1": "0") }; break; } case T_Code.Dir: { - if (_Controle.Angulo > 0) - { - _Controle.Direcao = Direcao.Direita; - } - else if (_Controle.Angulo < 0) - { - _Controle.Direcao = Direcao.Esquerda; - } - else - { - _Controle.Direcao = Direcao.Parado; - } - bool envioNecessario = ForcarEnvio || _Controle.Angulo != _ControleAnterior.Angulo || _Controle.TipoMovimento != _ControleAnterior.TipoMovimento; var modsAtualizar = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.Where(Mod => !Mod.DirMotor.AtingiuAnguloSP).Select(x => x.Modulo_ID).ToList(); if (!MKS057DCanService.Referenciando && (envioNecessario || modsAtualizar.Any())) { foreach (var Mod in Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.Where(x => envioNecessario ? true : modsAtualizar.Contains(x.Modulo_ID))) { - Mod.DirMotor.EnviarComandoControle(_Controle.Direcao); + Mod.DirMotor.EnviarComandoControle(DirecaoAtual); } - - _Controle.TiposControle.FirstOrDefault(x => x.Tipo == Tipo).UltimaDirecao = _Controle.Direcao; } + //Variaveis.MostrarLog($"Angulo SP alterado de {_ControleAnterior.Angulo:F2} para {_Controle.Angulo:F2}"); + _ControleAnterior.Angulo = _Controle.Angulo; - _ControleAnterior.Direcao = _Controle.Direcao; + _ControleAnterior.TipoMovimento = _Controle.TipoMovimento; RedisService.AtualizarCampos( CtxKey.DadosControle, @@ -1432,7 +1414,7 @@ namespace AgroBase.Models Protocolos = new List() { - _Controle.Direcao.ToString() + ";" + _Controle.Angulo.ToString() + ";" + _Controle.TipoMovimento.ToString() + ";" + (ForcarEnvio ? "1": "0") + DirecaoAtual.ToString() + ";" + _Controle.Angulo.ToString() + ";" + _Controle.TipoMovimento.ToString() + ";" + (ForcarEnvio ? "1": "0") }; break; } @@ -1469,6 +1451,8 @@ namespace AgroBase.Models } } + _Controle.TiposControle.FirstOrDefault(x => x.Tipo == Tipo).UltimaDirecao = DirecaoAtual; + return Protocolos; } @@ -1489,11 +1473,12 @@ namespace AgroBase.Models if (Sonar?.Resumo?.EnviarComandoParada ?? false) { Variaveis.MostrarLog("[OperacaoModel] Deve enviar o comando de parada em tmrLeituras_Tick"); - GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Mov, ForcarComando: true); + //GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Mov, ForcarComando: true); + GeneralJoystick.ProcessarDadosControle(BotoesJoystick.Xis, true); if (Variaveis.OperacaoEmAndamento.Parametros?.Controle?.FrenagemAutomaticaAoParar ?? false) { Variaveis.MostrarLog("[OperacaoModel] Deve enviar o comando de freio em tmrLeituras_Tick"); - GeneralJoystick.EnviaComandoMotor(Keys.Space, Disp: T_Code.Mov, ForcarComando: true); + GeneralJoystick.ProcessarDadosControle(BotoesJoystick.Bolinha, false); } } @@ -1502,13 +1487,13 @@ namespace AgroBase.Models var _Controle = Variaveis.OperacaoEmAndamento.Controle; if ((_Controle.TiposControle?.FirstOrDefault(x => x?.Tipo == T_Code.Mov)?.UltimaDirecao ?? Direcao.Parado) != Direcao.Parado) { - _Controle.PercentualVelocidadeSP = 0; - GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Mov, ForcarComando: true); + //GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Mov, ForcarComando: true); + GeneralJoystick.ProcessarDadosControle(BotoesJoystick.Xis, true); } if ((_Controle.TiposControle?.FirstOrDefault(x => x?.Tipo == T_Code.Dir)?.UltimaDirecao ?? Direcao.Parado) != Direcao.Parado) { - _Controle.Angulo = 0; - GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Dir, ForcarComando: true); + //GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Dir, ForcarComando: true); + GeneralJoystick.ProcessarDadosControle(BotoesJoystick.SEsquerda, true); } } } @@ -1683,7 +1668,8 @@ namespace AgroBase.Models if (controleBase.Dispositivo == T_Code.Dir && controleBase.Tecla == BotoesJoystick.R3) { - GeneralJoystick.EnviaComandoMotor(Keys.R, ForcarComando: true, ID: controleBase._comp_id); + //GeneralJoystick.EnviaComandoMotor(Keys.R, ForcarComando: true, ID: controleBase._comp_id); + GeneralJoystick.ProcessarDadosControle(BotoesJoystick.R3, false); return; } @@ -1720,14 +1706,22 @@ namespace AgroBase.Models if (cmdRover?.Dispositivo == T_Code.Dir && Variaveis.OperacaoEmAndamento.Parametros.Controle.DirecionalAutomatico && Variaveis.OperacaoEmAndamento.Iniciado) return; var _Controle = Variaveis.OperacaoEmAndamento.Controle; - int mx = cmdRover?.key == Keys.Left || cmdRover?.key == Keys.Down ? -1 : 1; - if (controleBase.Controle.PercentualVelocidadeSP != null) _Controle.PercentualVelocidadeSP = (double)controleBase.Controle.PercentualVelocidadeSP * mx; - if (controleBase.Controle.AnguloSP != null) _Controle.Angulo = (double)controleBase.Controle.AnguloSP * mx; + if (controleBase.Controle.PercentualVelocidadeSP != null) _Controle.PercentualVelocidadeSP = (double)controleBase.Controle.PercentualVelocidadeSP; + if (controleBase.Controle.AnguloSP != null) _Controle.Angulo = (double)controleBase.Controle.AnguloSP; if (controleBase.Controle.TipoMovimentoDirecional != null) _Controle.TipoMovimento = (TipoMovimentoDirecional)controleBase.Controle.TipoMovimentoDirecional; if (controleBase.Controle.EmFreio != null) _Controle.EmFreio = (bool)controleBase.Controle.EmFreio; - Keys tecla = cmdRover?.key ?? Keys.Escape; - GeneralJoystick.EnviaComandoMotor(tecla, controleBase.Dispositivo, ForcarComando: true, ID: controleBase._comp_id); + //GeneralJoystick.EnviaComandoMotor(tecla, controleBase.Dispositivo, ForcarComando: true, ID: controleBase._comp_id); + + switch (cmdRover?.Dispositivo) + { + case T_Code.Mov: + GeneralJoystick.ProcessarDadosControle(cmdRover.botaoJoy, controleBase.Solto, valorDesejado: _Controle.PercentualVelocidadeSP); + break; + case T_Code.Dir: + GeneralJoystick.ProcessarDadosControle(cmdRover.botaoJoy, controleBase.Solto, valorDesejado: _Controle.Angulo); + break; + } } private async Task tmrComunicacao_Tick() @@ -2146,7 +2140,18 @@ namespace AgroBase.Models return Convert.ToInt32(RPM_SP * VariaveisEquipamento.RelacaoRPM); } } - public double PercentualVelocidadeSP { get; set; } + private double _pctVelSp; + public double PercentualVelocidadeSP + { + get + { + return _pctVelSp; + } + set + { + _pctVelSp = value; + } + } public double PercentualVelocidadeSPKmh { get @@ -2155,7 +2160,7 @@ namespace AgroBase.Models } } public bool EmFreio { get; set; } = false; - public Direcao Direcao { get; set; } = Direcao.Parado; + //public Direcao Direcao { get; set; } = Direcao.Parado; public List Bicos => Variaveis.OperacaoEmAndamento.DispAtu?.Dados?.BicosPulverizadores; public double AlturaBarra { get; set; } private TipoMovimentoDirecional _tipoMovimento = TipoMovimentoDirecional.Diagnostico; @@ -2215,12 +2220,15 @@ namespace AgroBase.Models public void Reiniciar() { var _Parametros = Variaveis.OperacaoEmAndamento.Parametros.Controle; - PercentualVelocidadeSP = _Parametros.MovimentoAutomatico ? 0 : _Parametros.MovVelocidadeCErvasPercent; - Angulo = _Parametros.DirecionalAutomatico ? 0 : _Parametros.DirAnguloMaximo; + PercentualVelocidadeSP = 0; // _Parametros.MovimentoAutomatico ? 0 : _Parametros.MovVelocidadeCErvasPercent; + Angulo = 0; // _Parametros.DirecionalAutomatico ? 0 : _Parametros.DirAnguloMaximo; EmFreio = false; - Direcao = Direcao.Parado; TipoMovimento = TipoMovimentoDirecional.RodasDianteiras; - TiposControle.ForEach(x => x.UltimaDirecao = Direcao.Parado); + TiposControle.ForEach(x => + { + x.DirecaoAtual = Direcao.Parado; + x.UltimaDirecao = Direcao.Parado; + }); } public OperacaoControleModel Clone() @@ -2228,7 +2236,6 @@ namespace AgroBase.Models return new OperacaoControleModel() { Angulo = Angulo, - Direcao = Direcao, PercentualVelocidadeSP = PercentualVelocidadeSP, _tipoMovimento = _tipoMovimento, TipoMovimento = TipoMovimento, @@ -2254,6 +2261,7 @@ namespace AgroBase.Models public int DelayEnvioComandoExtra { get; set; } public DateTime UltimoComando { get; set; } public List Comandos { get; set; } + public Direcao DirecaoAtual { get; set; } = Direcao.Parado; public Direcao UltimaDirecao { get; set; } = Direcao.Parado; } @@ -2277,6 +2285,7 @@ namespace AgroBase.Models { public DateTime Momento { get; set; } public BotoesJoystick Tecla { get; set; } + public bool Solto { get; set; } public T_Code Dispositivo { get; set; } public bool Emergencia { get; set; } public bool Pausa { get; set; } diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index 769c3eddb..21320b9cd 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -1037,6 +1037,7 @@ namespace AgroBase.Models DateTime agora = DateTime.Now; var _Controle = Variaveis.OperacaoEmAndamento.Controle; + bool comandoMudou = novoComando != null && (_Controle.Angulo != novoComando.angulo || _Controle.PercentualVelocidadeSP != novoComando.percentual_velocidade || _Controle.TipoMovimento != novoComando.tipo_movimento || _Controle.EmFreio != novoComando.em_freio); bool mangerDestravou = novoComando != null && (_Controle.ticks_sem_resposta.AddMilliseconds(min_ticks_sem_resposta) < agora && novoComando.heartbeat != _Controle.heartbeat); if (novoComando != null && (comandoMudou || mangerDestravou)) @@ -1045,6 +1046,7 @@ namespace AgroBase.Models if (pControle.MovimentoAutomatico) { + _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov).DirecaoAtual = novoComando.percentual_velocidade > 0 ? Direcao.Frente : Direcao.Parado; _Controle.PercentualVelocidadeSP = novoComando.percentual_velocidade; _Controle.EmFreio = pControle.FrenagemAutomaticaAoParar && novoComando.em_freio; Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List() { T_Code.Mov }); @@ -1052,6 +1054,7 @@ namespace AgroBase.Models if (pControle.DirecionalAutomatico) { + _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir).DirecaoAtual = novoComando.angulo < 0 ? Direcao.Esquerda : novoComando.angulo > 0 ? Direcao.Direita : Direcao.Parado; _Controle.Angulo = novoComando.angulo; _Controle.TipoMovimento = novoComando.tipo_movimento; _Controle.SimulacaoMPC = novoComando.simulacao?.Select(x => new MPCSimulacaoModel() { latitude = x[0], longitude = x[1], orientacao = x[2] }).ToList() ?? new List(); @@ -1086,7 +1089,7 @@ namespace AgroBase.Models if (Variaveis.OperacaoEmAndamento?.Sensoriamento?.Controle?.RPM_SP > 0) { Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog(T_Code.Npc, StatusModulo.Falha, 0, "Muito tempo sem receber novo comando do Manager Worker! Parando Movimento!"); - Console.WriteLine("Muito tempo sem receber novo comando do Manager Worker! Parando Movimento!"); + Variaveis.MostrarLog("Muito tempo sem receber novo comando do Manager Worker! Parando Movimento!"); RedisService.AtualizarCampos( CtxKey.DadosControle, ("velocidade_sp", 0) @@ -1098,7 +1101,7 @@ namespace AgroBase.Models if (Variaveis.OperacaoEmAndamento?.Sensoriamento?.Controle?.RPM_SP > Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent) { Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog(T_Code.Npc, StatusModulo.Alerta, 80, "Muito tempo sem receber novo comando do Manager Worker! Reduzindo Velocidade!"); - Console.WriteLine("Muito tempo sem receber novo comando do Manager Worker! Reduzindo Velocidade!"); + Variaveis.MostrarLog("Muito tempo sem receber novo comando do Manager Worker! Reduzindo Velocidade!"); RedisService.AtualizarCampos( CtxKey.DadosControle, ("velocidade_sp", Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent) diff --git a/AgroBase/AgroBase/Models/UdpCtrlMessage.cs b/AgroBase/AgroBase/Models/UdpCtrlMessage.cs index 1477c9faa..93d219c4a 100644 --- a/AgroBase/AgroBase/Models/UdpCtrlMessage.cs +++ b/AgroBase/AgroBase/Models/UdpCtrlMessage.cs @@ -19,13 +19,14 @@ public sealed class UdpCtrlMessage public byte Device; public UdpCtrlFlags Flags; public BotoesJoystick Key; + public bool released; public short P1; public short P2; public byte[] ToBytes() { - // 10 bytes fixos - var buf = new byte[10]; + // 11 bytes fixos + var buf = new byte[11]; buf[0] = Version; buf[1] = Device; buf[2] = (byte)Flags; @@ -35,16 +36,19 @@ public sealed class UdpCtrlMessage buf[3] = (byte)(k >> 8); buf[4] = (byte)(k & 0xFF); + // botao solto + buf[5] = (byte)(released ? 1 : 0); + // p1 int16 BE - buf[5] = (byte)((ushort)P1 >> 8); - buf[6] = (byte)((ushort)P1 & 0xFF); + buf[6] = (byte)((ushort)P1 >> 8); + buf[7] = (byte)((ushort)P1 & 0xFF); // p2 int16 BE - buf[7] = (byte)((ushort)P2 >> 8); - buf[8] = (byte)((ushort)P2 & 0xFF); + buf[8] = (byte)((ushort)P2 >> 8); + buf[9] = (byte)((ushort)P2 & 0xFF); // reservado (checksum simples futuro / seq local etc) - buf[9] = 0; + buf[10] = 0; return buf; } @@ -64,8 +68,10 @@ public sealed class UdpCtrlMessage ushort k = (ushort)((payload[3] << 8) | payload[4]); m.Key = (BotoesJoystick)k; - m.P1 = (short)((payload[5] << 8) | payload[6]); - m.P2 = (short)((payload[7] << 8) | payload[8]); + m.released = payload[5] == 1; + + m.P1 = (short)((payload[6] << 8) | payload[7]); + m.P2 = (short)((payload[8] << 8) | payload[9]); msg = m; return true; diff --git a/AgroBase/AgroBase/Models/Variaveis.cs b/AgroBase/AgroBase/Models/Variaveis.cs index 2397db831..826538ec1 100644 --- a/AgroBase/AgroBase/Models/Variaveis.cs +++ b/AgroBase/AgroBase/Models/Variaveis.cs @@ -276,6 +276,7 @@ namespace AgroBase.Models Emergencia = emergencia, Pausa = pausa, Tecla = msg.Key, + Solto = msg.released, Dispositivo = disp, Controle = new OperacaoComandoBaseControleModel(), }; @@ -314,8 +315,10 @@ namespace AgroBase.Models if (ms > FAILSAFE_MS) { // Para por segurança - GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Mov, ForcarComando: true, ID: null); - GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Dir, ForcarComando: true, ID: null); + //GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Mov, ForcarComando: true, ID: null); + //GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Dir, ForcarComando: true, ID: null); + GeneralJoystick.ProcessarDadosControle(BotoesJoystick.Xis, true); + GeneralJoystick.ProcessarDadosControle(BotoesJoystick.SEsquerda, true); // desarma até chegar novo comando válido Interlocked.Exchange(ref _lastValidCmdTicksUtc, 0); diff --git a/AgroBase/AgroBase/Services/LoRaSerializer.cs b/AgroBase/AgroBase/Services/LoRaSerializer.cs index 2aab08a11..8e7ae8694 100644 --- a/AgroBase/AgroBase/Services/LoRaSerializer.cs +++ b/AgroBase/AgroBase/Services/LoRaSerializer.cs @@ -292,7 +292,7 @@ namespace AgroBase.Services { List dados = new List(); dados.AddRange(CanPrefix(DadosLoRaParse.DadosControle)); - dados.Add((byte)Controle.Direcao); // 1 byte + dados.Add((byte)Direcao.Parado); // 1 byte dados.Add((byte)Controle.PercentualVelocidadeSP); // 1 byte dados.Add((byte)Controle.TipoMovimento); // 1 byte dados.Add((byte)Controle.Angulo); // 1 byte diff --git a/AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs b/AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs index baf5335c9..6b8c3842b 100644 --- a/AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs +++ b/AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs @@ -91,6 +91,7 @@ namespace AgroBase.Services.Operadores var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle; if (pControle.MovimentoAutomatico) { + _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov).DirecaoAtual = novoComando.percentual_velocidade > 0 ? Direcao.Frente : Direcao.Parado; _Controle.PercentualVelocidadeSP = novoComando.percentual_velocidade; _Controle.EmFreio = pControle.FrenagemAutomaticaAoParar && novoComando.em_freio; Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List() { T_Code.Mov }); @@ -98,6 +99,7 @@ namespace AgroBase.Services.Operadores if (pControle.DirecionalAutomatico) { + _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir).DirecaoAtual = novoComando.angulo < 0 ? Direcao.Esquerda : novoComando.angulo > 0 ? Direcao.Direita : Direcao.Parado; _Controle.Angulo = novoComando.angulo; _Controle.TipoMovimento = novoComando.tipo_movimento; _Controle.SimulacaoMPC = novoComando.simulacao?.Select(x => new MPCSimulacaoModel() { latitude = x[0], longitude = x[1], orientacao = x[2] }).ToList() ?? new List(); diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py index c1e8fc8c3..c389f02df 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py @@ -961,7 +961,7 @@ class ControladorMPC: ponto_alvo_primario = self.pontos_info[idx_alvo_correcao]["xy"] erro_lateral, _, _, _, theta_path, _ = self.cross_track_error_point(x, y) _, erro_orientacao = self._calcula_erro_orientacao(x, y, theta, ponto_alvo_primario, theta_path, 0.5) - + _cmd = { "enviar_comando": True, "parada_necessaria": parada_necessaria, diff --git a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py index c58c56d26..ce43d0e93 100644 --- a/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py +++ b/AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py @@ -60,7 +60,7 @@ class ProcessadorEmAndamento(ProcessadorBase): if direcional_automatico: comando_dir = definir_comando_dir(self.pid_dir, envio_necessario) - enviar_comando = comando_dir.get("enviar_comando", False) or movimento_automatico + enviar_comando = comando_dir.get("enviar_comando", False) # or movimento_automatico if enviar_comando: self.ultimo_comando_enviado = agora diff --git a/AgroBase/OperationControl/Models/Variaveis.cs b/AgroBase/OperationControl/Models/Variaveis.cs index 317bc9adf..ee3d803c3 100644 --- a/AgroBase/OperationControl/Models/Variaveis.cs +++ b/AgroBase/OperationControl/Models/Variaveis.cs @@ -453,13 +453,14 @@ namespace OperationControl.Models } } - public static void EnviarComandoDirecional(AgroBase.Models.Enums.Direcao direcao, double angulo, AgroBase.Models.Enums.TipoMovimentoDirecional movimento, string mod_id = null) + public static void EnviarComandoDirecional(AgroBase.Models.Enums.BotoesJoystick botao, bool solto, double angulo, AgroBase.Models.Enums.TipoMovimentoDirecional movimento, string mod_id = null) { EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel() { Momento = DateTime.Now, Dispositivo = AgroBase.Models.Enums.T_Code.Dir, - Tecla = DeParaDirecaoTecla(direcao), + Tecla = botao, + Solto = solto, _comp_id = mod_id, Controle = new OperacaoComandoBaseControleModel() { @@ -469,15 +470,14 @@ namespace OperationControl.Models }); } - public static void EnviarComandoDirecionalUDP(AgroBase.Models.Enums.Direcao direcao, double angulo, AgroBase.Models.Enums.TipoMovimentoDirecional tipoMov) + public static void EnviarComandoDirecionalUDP(AgroBase.Models.Enums.BotoesJoystick botao, bool solto, double angulo, AgroBase.Models.Enums.TipoMovimentoDirecional tipoMov) { - var key = DeParaDirecaoTecla(direcao); - var msg = new UdpCtrlMessage { Device = (byte)AgroBase.Models.Enums.T_Code.Dir, Flags = UdpCtrlFlags.HasKey | UdpCtrlFlags.HasPayload, - Key = key, + Key = botao, + released = solto, P1 = (short)Math.Max(short.MinValue, Math.Min(short.MaxValue, (int)Math.Round(angulo * 100.0))), P2 = (short)tipoMov }; @@ -486,13 +486,14 @@ namespace OperationControl.Models Variaveis.ControlSenderDir.Update(msg); } - public static void EnviarComandoMovimentacao(AgroBase.Models.Enums.Direcao direcao, double velocidade) + public static void EnviarComandoMovimentacao(AgroBase.Models.Enums.BotoesJoystick botao, bool solto, double velocidade) { EnviarDadosControle(SelectedRoverId, new OperacaoComandoBaseModel() { Momento = DateTime.Now, Dispositivo = AgroBase.Models.Enums.T_Code.Mov, - Tecla = DeParaDirecaoTecla(direcao), + Tecla = botao, + Solto = solto, Controle = new OperacaoComandoBaseControleModel() { PercentualVelocidadeSP = velocidade @@ -500,15 +501,14 @@ namespace OperationControl.Models }); } - public static void EnviarComandoMovimentacaoUDP(AgroBase.Models.Enums.Direcao direcao, double velPct, bool emfreio) + public static void EnviarComandoMovimentacaoUDP(AgroBase.Models.Enums.BotoesJoystick botao, bool solto, double velPct, bool emfreio) { - var key = DeParaDirecaoTecla(direcao); - var msg = new UdpCtrlMessage { Device = (byte)AgroBase.Models.Enums.T_Code.Mov, Flags = UdpCtrlFlags.HasKey | UdpCtrlFlags.HasPayload, - Key = key, + Key = botao, + released = solto, P1 = (short)Math.Max(short.MinValue, Math.Min(short.MaxValue, (int)Math.Round(velPct * 100.0))), P2 = (short)(emfreio ? 1 : 0) };