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

ajustes no controle

This commit is contained in:
Diego Freitas 2026-03-23 17:08:01 -03:00
parent ee9a93837a
commit de5f1440f2
17 changed files with 396 additions and 273 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
AgroBase/AgroBase/Forms/IHM/frmDiagnosticos.cs
View File

@ -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;

17
AgroBase/AgroBase/Forms/IHM/frmDialogoMapa.cs
View File

@ -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);
}
}

2
AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs
View File

@ -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}";

17
AgroBase/AgroBase/Forms/frmJoystick.cs
View File

@ -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);
}
}

8
AgroBase/AgroBase/Forms/frmOID.cs
View File

@ -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
{

2
AgroBase/AgroBase/Forms/frmPrincipal.cs
View File

@ -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);

388
AgroBase/AgroBase/Models/GeneralJoystick.cs
View File

@ -22,22 +22,18 @@ namespace AgroBase
public static List<ComandosDispositivo> Comandos = new List<ComandosDispositivo>() { };
public static List<DeParaComandos> DeParaComandos = new List<DeParaComandos>()
{
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<BotoesJoystick> BotoesJoyPressionados = new List<BotoesJoystick>() { };
public static List<TipoMovimentoDirecional> MovimentosCombinados = new List<TipoMovimentoDirecional>()
@ -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<T_Code>() { 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<string> modsControlar = (!string.IsNullOrEmpty(ID)) ? new List<string>() { ID } : null;
Task.Run(async () => await Variaveis.OperacaoEmAndamento.DispMvd.Dados.RealizarReferenciamentoDirecional(ModsReferenciar: modsControlar));
}
else
{
_Controle.Angulo = Comando.ValorAplicar;
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(ForcarComando, new List<T_Code>() { 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; }
}
}

2
AgroBase/AgroBase/Models/Modules/MovimentacaoModel.cs
View File

@ -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;

159
AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs
View File

@ -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<OperacaoControleTipoModel>()
{
@ -455,7 +454,9 @@ namespace AgroBase.Models
DelayEnvioComando = 250,
DelayEnvioComandoExtra = 0,
Comandos = new List<string>(),
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<string>(),
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<string>(),
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<string>(),
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<OperacaoControleTipoModel>()
@ -723,7 +728,9 @@ namespace AgroBase.Models
DelayEnvioComando = 500,
DelayEnvioComandoExtra = 0,
Comandos = new List<string>(),
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<string>(),
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<OperacaoControleTipoModel>()
@ -826,7 +834,9 @@ namespace AgroBase.Models
DelayEnvioComando = 500,
DelayEnvioComandoExtra = 0,
Comandos = new List<string>(),
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<string>(),
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<string>()
{
_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<string>()
{
_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<AtuadorBicoModel> 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<string> 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; }

7
AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs
View File

@ -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>() { 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<MPCSimulacaoModel>();
@ -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)

24
AgroBase/AgroBase/Models/UdpCtrlMessage.cs
View File

@ -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;

7
AgroBase/AgroBase/Models/Variaveis.cs
View File

@ -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);

2
AgroBase/AgroBase/Services/LoRaSerializer.cs
View File

@ -292,7 +292,7 @@ namespace AgroBase.Services
{
List<byte> dados = new List<byte>();
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

2
AgroBase/AgroBase/Services/Operadores/ManagerWorkerService.cs
View File

@ -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>() { 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<MPCSimulacaoModel>();

2
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py
View File

@ -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

24
AgroBase/OperationControl/Models/Variaveis.cs
View File

@ -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)
};