From 708cc5c80a866a5d410c60844e5aa56368f2ce6e Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Fri, 17 Feb 2023 01:34:36 -0300 Subject: [PATCH 01/19] [MP-2]: Added button for Create Maneuvers, fixed some UI --- .gitignore | 3 +- .../controllers/LiftoffController.java | 2 +- src/com/pesterenan/model/ActiveVessel.java | 2 +- .../resources/MechPesteBundle.properties | 3 +- .../resources/MechPesteBundle_pt.properties | 3 +- src/com/pesterenan/utils/Modulos.java | 47 ++--- .../views/FunctionsAndTelemetryJPanel.java | 23 ++- src/com/pesterenan/views/MainGui.java | 166 +++++++++--------- src/com/pesterenan/views/ManeuverJPanel.java | 2 +- 9 files changed, 128 insertions(+), 123 deletions(-) diff --git a/.gitignore b/.gitignore index c01385e..e9ea9a6 100644 --- a/.gitignore +++ b/.gitignore @@ -11,4 +11,5 @@ tmp/ local.properties .loadpath .metadata -workspace.xml \ No newline at end of file +workspace.xml +settings.json \ No newline at end of file diff --git a/src/com/pesterenan/controllers/LiftoffController.java b/src/com/pesterenan/controllers/LiftoffController.java index dcc48d9..b565948 100644 --- a/src/com/pesterenan/controllers/LiftoffController.java +++ b/src/com/pesterenan/controllers/LiftoffController.java @@ -126,7 +126,7 @@ private void finalizeCurve() throws RPCException, StreamException, InterruptedEx private void circularizeOrbitOnApoapsis() { setCurrentStatus(Bundle.getString("status_planning_orbit")); Map commands = new HashMap<>(); - commands.put(Modulos.MODULO.get(), Modulos.MODULO_MANOBRAS.get()); + commands.put(Modulos.MODULO.get(), Modulos.MODULO_EXEC_MANOBRAS.get()); commands.put(Modulos.FUNCAO.get(), Modulos.APOASTRO.get()); commands.put(Modulos.AJUSTE_FINO.get(), String.valueOf(false)); MechPeste.newInstance().startModule(commands); diff --git a/src/com/pesterenan/model/ActiveVessel.java b/src/com/pesterenan/model/ActiveVessel.java index 7d858d6..2fd88d1 100644 --- a/src/com/pesterenan/model/ActiveVessel.java +++ b/src/com/pesterenan/model/ActiveVessel.java @@ -182,7 +182,7 @@ public void startModule(Map commands) { controller = new LandingController(commands); runningModule = true; } - if (currentFunction.equals(Modulos.MODULO_MANOBRAS.get())) { + if (currentFunction.equals(Modulos.MODULO_EXEC_MANOBRAS.get())) { controller = new ManeuverController(commands); runningModule = true; } diff --git a/src/com/pesterenan/resources/MechPesteBundle.properties b/src/com/pesterenan/resources/MechPesteBundle.properties index a189d31..96915a6 100644 --- a/src/com/pesterenan/resources/MechPesteBundle.properties +++ b/src/com/pesterenan/resources/MechPesteBundle.properties @@ -1,6 +1,7 @@ btn_func_landing = Auto Landing btn_func_liftoff = Orbital Liftoff -btn_func_maneuvers = Exec. Maneuvers +btn_func_create_maneuver = Create Maneuvers +btn_func_exec_maneuver = Exec. Maneuvers btn_func_rover = Drive Rover btn_stat_connect = Connect installer_dialog_btn_browse = Browse... diff --git a/src/com/pesterenan/resources/MechPesteBundle_pt.properties b/src/com/pesterenan/resources/MechPesteBundle_pt.properties index 4cf85c2..814c3b2 100644 --- a/src/com/pesterenan/resources/MechPesteBundle_pt.properties +++ b/src/com/pesterenan/resources/MechPesteBundle_pt.properties @@ -1,6 +1,7 @@ btn_func_landing = Pouso Autom\u00E1tico btn_func_liftoff = Decolagem Orbital -btn_func_maneuvers = Exec. Manobras +btn_func_create_maneuver = Criar Manobras +btn_func_exec_maneuver = Exec. Manobras btn_func_rover = Pilotar Rover btn_stat_connect = Conectar installer_dialog_btn_browse = Escolher... diff --git a/src/com/pesterenan/utils/Modulos.java b/src/com/pesterenan/utils/Modulos.java index e686230..85222ba 100644 --- a/src/com/pesterenan/utils/Modulos.java +++ b/src/com/pesterenan/utils/Modulos.java @@ -2,39 +2,40 @@ public enum Modulos { - APOASTRO("Apoastro"), - PERIASTRO("Periastro"), - EXECUTAR("Executar"), + ABRIR_PAINEIS("Abrir Painéis"), AJUSTAR("Ajustar"), + AJUSTE_FINO("Ajuste Fino"), + ALTITUDE_SOBREVOO("Altitude Sobrevoo"), + APOASTRO("Apoastro"), + CIRCULAR("Circular"), + CUBICA("Cúbica"), DIRECAO("Direção"), - MODULO("Módulo"), + EXECUTAR("Executar"), + EXPONENCIAL("Exponencial"), FUNCAO("Função"), + INCLINACAO("Inclinação"), + MARCADOR_MAPA("Marcador no mapa"), + MAX_TWR("Max_TWR"), + MODULO_CRIAR_MANOBRAS("CRIAR_MANOBRAS"), MODULO_DECOLAGEM("LIFTOFF"), - MODULO_MANOBRAS("MANEUVER"), + MODULO_EXEC_MANOBRAS("MANEUVER"), + MODULO_POUSO_SOBREVOAR("HOVER"), MODULO_POUSO("LANDING"), MODULO_ROVER("ROVER"), MODULO_TELEMETRIA("TELEMETRY"), - MODULO_POUSO_SOBREVOAR("HOVER"), - ALTITUDE_SOBREVOO("Altitude Sobrevoo"), - INCLINACAO("Inclinação"), - CIRCULAR("Circular"), - QUADRATICA("Quadrática"), - CUBICA("Cúbica"), - SINUSOIDAL("Sinusoidal"), - EXPONENCIAL("Exponencial"), - ROLAGEM("Rolagem"), - USAR_ESTAGIOS("Usar Estágios"), - ABRIR_PAINEIS("Abrir Painéis"), - AJUSTE_FINO("Ajuste Fino"), - TIPO_ALVO_ROVER("Tipo de Alvo do Rover"), + MODULO("Módulo"), NAVE_ALVO("Nave alvo"), - MARCADOR_MAPA("Marcador no mapa"), NOME_MARCADOR("Nome do marcador"), - VELOCIDADE_MAX("Velocidade Máxima"), - POUSAR("Pousar nave"), - MAX_TWR("Max_TWR"), ORBITA_BAIXA("ÓRBITA BAIXA"), - SOBREVOO_POS_POUSO("SOBREVOO PÓS POUSO"); + PERIASTRO("Periastro"), + POUSAR("Pousar nave"), + QUADRATICA("Quadrática"), + ROLAGEM("Rolagem"), + SINUSOIDAL("Sinusoidal"), + SOBREVOO_POS_POUSO("SOBREVOO PÓS POUSO"), + TIPO_ALVO_ROVER("Tipo de Alvo do Rover"), + USAR_ESTAGIOS("Usar Estágios"), + VELOCIDADE_MAX("Velocidade Máxima"); final String t; diff --git a/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java b/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java index bce16a3..3aa27b5 100644 --- a/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java +++ b/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java @@ -21,7 +21,7 @@ public class FunctionsAndTelemetryJPanel extends JPanel implements UIMethods { private static final long serialVersionUID = 0L; private final Dimension btnFuncDimension = new Dimension(140, 25); - private JButton btnLiftoff, btnLanding, btnManeuver, btnRover, btnCancel; + private JButton btnLiftoff, btnLanding, btnExecManeuver, btnRover, btnCancel, btnCreateManeuver; private static JLabel lblAltitude, lblSurfaceAlt, lblApoapsis, lblPeriapsis, lblVertSpeed, lblHorzSpeed; private static JLabel lblAltitudeValue, lblSurfaceAltValue, lblApoapsisValue; private static JLabel lblPeriapsisValue, lblVertSpeedValue, lblHorzSpeedValue; @@ -51,7 +51,8 @@ public void initComponents() { // Buttons: btnLiftoff = new JButton(Bundle.getString("btn_func_liftoff")); btnLanding = new JButton(Bundle.getString("btn_func_landing")); - btnManeuver = new JButton(Bundle.getString("btn_func_maneuvers")); + btnCreateManeuver = new JButton(Bundle.getString("btn_func_create_maneuver")); + btnExecManeuver = new JButton(Bundle.getString("btn_func_exec_maneuver")); btnRover = new JButton(Bundle.getString("btn_func_rover")); btnCancel = new JButton(Bundle.getString("pnl_tel_btn_cancel")); } @@ -75,10 +76,14 @@ public void setupComponents() { btnLiftoff.setActionCommand(Modulos.MODULO_DECOLAGEM.get()); btnLiftoff.setMaximumSize(btnFuncDimension); btnLiftoff.setPreferredSize(btnFuncDimension); - btnManeuver.addActionListener(this::changeFunctionPanel); - btnManeuver.setActionCommand(Modulos.MODULO_MANOBRAS.get()); - btnManeuver.setMaximumSize(btnFuncDimension); - btnManeuver.setPreferredSize(btnFuncDimension); + btnCreateManeuver.addActionListener(this::changeFunctionPanel); + btnCreateManeuver.setActionCommand(Modulos.MODULO_CRIAR_MANOBRAS.get()); + btnCreateManeuver.setMaximumSize(btnFuncDimension); + btnCreateManeuver.setPreferredSize(btnFuncDimension); + btnExecManeuver.addActionListener(this::changeFunctionPanel); + btnExecManeuver.setActionCommand(Modulos.MODULO_EXEC_MANOBRAS.get()); + btnExecManeuver.setMaximumSize(btnFuncDimension); + btnExecManeuver.setPreferredSize(btnFuncDimension); btnRover.addActionListener(this::changeFunctionPanel); btnRover.setActionCommand(Modulos.MODULO_ROVER.get()); btnRover.setMaximumSize(btnFuncDimension); @@ -93,8 +98,10 @@ public void layoutComponents() { pnlFunctionControls.add(MainGui.createMarginComponent(0, 4)); pnlFunctionControls.add(btnLiftoff); pnlFunctionControls.add(btnLanding); - pnlFunctionControls.add(btnManeuver); pnlFunctionControls.add(btnRover); + pnlFunctionControls.add(MainGui.createMarginComponent(0, 20)); + pnlFunctionControls.add(btnCreateManeuver); + pnlFunctionControls.add(btnExecManeuver); pnlFunctionControls.add(Box.createVerticalGlue()); JPanel pnlLeftPanel = new JPanel(); @@ -159,7 +166,7 @@ public void layoutComponents() { } private void changeFunctionPanel(ActionEvent e) { - MainGui.getCardJPanels().firePropertyChange(e.getActionCommand(), false, true); + MainGui.changeToPage(e); } public static void updateTelemetry(Map telemetryData) { diff --git a/src/com/pesterenan/views/MainGui.java b/src/com/pesterenan/views/MainGui.java index 7ceea6f..04c903a 100644 --- a/src/com/pesterenan/views/MainGui.java +++ b/src/com/pesterenan/views/MainGui.java @@ -12,105 +12,120 @@ import java.beans.PropertyChangeEvent; import java.beans.PropertyChangeListener; -public class MainGui extends JFrame implements ActionListener, PropertyChangeListener { +public class MainGui extends JFrame implements ActionListener, UIMethods { + private static final long serialVersionUID = 1L; + private final Dimension APP_DIMENSION = new Dimension(480, 300); public static final Dimension PNL_DIMENSION = new Dimension(464, 216); public static final Dimension BTN_DIMENSION = new Dimension(110, 25); - public static final EmptyBorder MARGIN_BORDER_10_PX_LR = new EmptyBorder(0,10,0,10); + public static final EmptyBorder MARGIN_BORDER_10_PX_LR = new EmptyBorder(0, 10, 0, 10); private static MainGui mainGui = null; private static StatusJPanel pnlStatus; private static FunctionsAndTelemetryJPanel pnlFunctionsAndTelemetry; private final JPanel ctpMainGui = new JPanel(); private final static JPanel cardJPanels = new JPanel(); private JMenuBar menuBar; - private JMenu mnFile; - private JMenu mnOptions; - private JMenu mnHelp; - private JMenuItem mntmInstallKrpc; - private JMenuItem mntmExit; - private JMenuItem mntmChangeVessels; - private JMenuItem mntmAbout; + private JMenu mnFile, mnOptions, mnHelp; + private JMenuItem mntmInstallKrpc, mntmExit, mntmChangeVessels, mntmAbout; + + private final static CardLayout cardLayout = new CardLayout(0, 0); private LiftoffJPanel pnlLiftoff; - - private final CardLayout cardLayout = new CardLayout(0, 0); private LandingJPanel pnlLanding; private ManeuverJPanel pnlManeuver; private RoverJPanel pnlRover; private MainGui() { - try { - UIManager.setLookAndFeel(UIManager.getSystemLookAndFeelClassName()); - initComponents(); - } catch (Throwable e) { - e.printStackTrace(); - } + initComponents(); + setupComponents(); + layoutComponents(); } - public static void newInstance() { + public static MainGui newInstance() { if (mainGui == null) { mainGui = new MainGui(); } + return mainGui; } - private void initComponents() { - setAlwaysOnTop(true); - setTitle("MechPeste - Pesterenan"); //$NON-NLS-1$ - setVisible(true); - setResizable(false); - setLocation(100, 100); - setSize(APP_DIMENSION); - + @Override + public void initComponents() { + try { + UIManager.setLookAndFeel(UIManager.getSystemLookAndFeelClassName()); + } catch (Exception ignored) { + } + + // Menu bar menuBar = new JMenuBar(); - setJMenuBar(menuBar); - - mnFile = new JMenu(Bundle.getString("main_mn_file")); //$NON-NLS-1$ - menuBar.add(mnFile); - - mnOptions = new JMenu(Bundle.getString("main_mn_options")); //$NON-NLS-1$ - menuBar.add(mnOptions); + // Menus + mnFile = new JMenu(Bundle.getString("main_mn_file")); + mnOptions = new JMenu(Bundle.getString("main_mn_options")); + mnHelp = new JMenu(Bundle.getString("main_mn_help")); + + // Menu Items mntmInstallKrpc = new JMenuItem(Bundle.getString("main_mntm_install_krpc")); - mntmInstallKrpc.addActionListener(this); - mnFile.add(mntmInstallKrpc); - mntmChangeVessels = new JMenuItem(Bundle.getString("main_mntm_change_vessels")); - mntmChangeVessels.addActionListener(this); - mnOptions.add(mntmChangeVessels); - - mnFile.add(new JSeparator()); - mntmExit = new JMenuItem(Bundle.getString("main_mntm_exit")); //$NON-NLS-1$ - mntmExit.addActionListener(this); - mnFile.add(mntmExit); - - mnHelp = new JMenu(Bundle.getString("main_mn_help")); //$NON-NLS-1$ - menuBar.add(mnHelp); - - mntmAbout = new JMenuItem(Bundle.getString("main_mntm_about")); //$NON-NLS-1$ - mntmAbout.addActionListener(this); - mnHelp.add(mntmAbout); - setContentPane(ctpMainGui); - setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); + mntmExit = new JMenuItem(Bundle.getString("main_mntm_exit")); + mntmAbout = new JMenuItem(Bundle.getString("main_mntm_about")); + // Panels pnlFunctionsAndTelemetry = new FunctionsAndTelemetryJPanel(); pnlLiftoff = new LiftoffJPanel(); pnlLanding = new LandingJPanel(); pnlManeuver = new ManeuverJPanel(); pnlRover = new RoverJPanel(); pnlStatus = new StatusJPanel(); + } - cardJPanels.setLayout(cardLayout); + @Override + public void setupComponents() { + // Main Panel setup: + setAlwaysOnTop(true); + setTitle("MechPeste - Pesterenan"); + setJMenuBar(menuBar); + setResizable(false); + setLocation(100, 100); + setContentPane(ctpMainGui); + setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); + + // Setting-up components: + mntmAbout.addActionListener(this); + mntmChangeVessels.addActionListener(this); + mntmExit.addActionListener(this); + mntmInstallKrpc.addActionListener(this); + + cardJPanels.setPreferredSize(PNL_DIMENSION); cardJPanels.setSize(PNL_DIMENSION); + } + + @Override + public void layoutComponents() { + // Main Panel layout: + setPreferredSize(APP_DIMENSION); + setSize(APP_DIMENSION); + setVisible(true); + + // Laying out components: + ctpMainGui.setLayout(new BorderLayout()); + ctpMainGui.add(cardJPanels, BorderLayout.CENTER); + ctpMainGui.add(pnlStatus, BorderLayout.SOUTH); + + mnFile.add(mntmInstallKrpc); + mnFile.add(new JSeparator()); + mnFile.add(mntmExit); + mnOptions.add(mntmChangeVessels); + mnHelp.add(mntmAbout); + menuBar.add(mnFile); + menuBar.add(mnOptions); + menuBar.add(mnHelp); + + cardJPanels.setLayout(cardLayout); cardJPanels.add(pnlFunctionsAndTelemetry, Modulos.MODULO_TELEMETRIA.get()); cardJPanels.add(pnlLiftoff, Modulos.MODULO_DECOLAGEM.get()); cardJPanels.add(pnlLanding, Modulos.MODULO_POUSO.get()); - cardJPanels.add(pnlManeuver, Modulos.MODULO_MANOBRAS.get()); + cardJPanels.add(pnlManeuver, Modulos.MODULO_EXEC_MANOBRAS.get()); cardJPanels.add(pnlRover, Modulos.MODULO_ROVER.get()); - cardJPanels.addPropertyChangeListener(this); - - ctpMainGui.setLayout(new BorderLayout(0, 0)); - ctpMainGui.add(cardJPanels, BorderLayout.CENTER); - ctpMainGui.add(pnlStatus, BorderLayout.SOUTH); } public void actionPerformed(ActionEvent e) { @@ -140,13 +155,6 @@ protected void handleMntmExitActionPerformed(ActionEvent e) { System.exit(0); } - @Override - public void propertyChange(PropertyChangeEvent evt) { - if (evt.getSource() == cardJPanels) { - handlePnlTelemetriaPropertyChange(evt); - } - } - public static Rectangle centerDialogOnScreen() { Dimension SCREEN_DIMENSIONS = Toolkit.getDefaultToolkit().getScreenSize(); Dimension DIALOG_DIMENSIONS = new Dimension(400, 240); @@ -157,30 +165,16 @@ public static Rectangle centerDialogOnScreen() { return new Rectangle(x, y, w, h); } - protected void handlePnlTelemetriaPropertyChange(PropertyChangeEvent evt) { - if (evt.getPropertyName().equals(Modulos.MODULO_DECOLAGEM.get())) { - cardLayout.show(cardJPanels, Modulos.MODULO_DECOLAGEM.get()); - } - if (evt.getPropertyName().equals(Modulos.MODULO_POUSO.get())) { - cardLayout.show(cardJPanels, Modulos.MODULO_POUSO.get()); - } - if (evt.getPropertyName().equals(Modulos.MODULO_MANOBRAS.get())) { - cardLayout.show(cardJPanels, Modulos.MODULO_MANOBRAS.get()); - } - if (evt.getPropertyName().equals(Modulos.MODULO_ROVER.get())) { - cardLayout.show(cardJPanels, Modulos.MODULO_ROVER.get()); - } - if (evt.getPropertyName().equals(Modulos.MODULO_TELEMETRIA.get())) { - cardLayout.show(cardJPanels, Modulos.MODULO_TELEMETRIA.get()); - } - } - public static JPanel getCardJPanels() { return cardJPanels; } + public static void changeToPage(ActionEvent e) { + cardLayout.show(cardJPanels, e.getActionCommand()); + } + public static void backToTelemetry(ActionEvent e) { - cardJPanels.firePropertyChange(Modulos.MODULO_TELEMETRIA.get(), false, true); + cardLayout.show(cardJPanels, Modulos.MODULO_TELEMETRIA.get()); } protected void handleMntmAboutActionPerformed(ActionEvent e) { @@ -188,7 +182,7 @@ protected void handleMntmAboutActionPerformed(ActionEvent e) { } public static Component createMarginComponent(int width, int height) { - Component marginComp = Box.createRigidArea(new Dimension(width,height)); + Component marginComp = Box.createRigidArea(new Dimension(width, height)); return marginComp; } } diff --git a/src/com/pesterenan/views/ManeuverJPanel.java b/src/com/pesterenan/views/ManeuverJPanel.java index d0e29ca..c0b80e9 100644 --- a/src/com/pesterenan/views/ManeuverJPanel.java +++ b/src/com/pesterenan/views/ManeuverJPanel.java @@ -171,7 +171,7 @@ public void actionPerformed(ActionEvent e) { protected void handleManeuverFunction(String maneuverFunction) { Map commands = new HashMap<>(); - commands.put(Modulos.MODULO.get(), Modulos.MODULO_MANOBRAS.get()); + commands.put(Modulos.MODULO.get(), Modulos.MODULO_EXEC_MANOBRAS.get()); commands.put(Modulos.FUNCAO.get(), maneuverFunction); commands.put(Modulos.AJUSTE_FINO.get(), String.valueOf(chkFineAdjusment.isSelected())); MechPeste.newInstance().startModule(commands); From b1604f9be9dbb63dba18d527cc691ec69cba67fe Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Wed, 22 Feb 2023 00:09:52 -0300 Subject: [PATCH 02/19] [MP-2]: Created button for Create Maneuvers Panel Included a fix for not finishing the maneuver burn. Changed the 'Change Vessel' area to 10km for 'nearest'. --- src/com/pesterenan/MechPeste.java | 4 ++-- src/com/pesterenan/controllers/ManeuverController.java | 4 +--- src/com/pesterenan/views/ChangeVesselDialog.java | 2 +- src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java | 1 + src/com/pesterenan/views/LiftoffJPanel.java | 2 +- 5 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/com/pesterenan/MechPeste.java b/src/com/pesterenan/MechPeste.java index 8460188..4f6fc12 100644 --- a/src/com/pesterenan/MechPeste.java +++ b/src/com/pesterenan/MechPeste.java @@ -74,7 +74,7 @@ private static boolean filterVessels(Vessel vessel, String search) { if (search == "all") { return true; } - double TWO_KILOMETERS = 2000.0; + double TEN_KILOMETERS = 10000.0; try { Vessel active = MechPeste.getSpaceCenter().getActiveVessel(); if (vessel.getOrbit().getBody().getName().equals(active.getOrbit().getBody().getName())) { @@ -83,7 +83,7 @@ private static boolean filterVessels(Vessel vessel, String search) { final double distance = Vector.distance(activePos, vesselPos); switch (search) { case "closest": - if (distance < TWO_KILOMETERS) { + if (distance < TEN_KILOMETERS) { return true; } break; diff --git a/src/com/pesterenan/controllers/ManeuverController.java b/src/com/pesterenan/controllers/ManeuverController.java index 9798dcf..398c2d9 100644 --- a/src/com/pesterenan/controllers/ManeuverController.java +++ b/src/com/pesterenan/controllers/ManeuverController.java @@ -334,19 +334,17 @@ public void executeBurn(Node noDeManobra, double duracaoDaQueima) { Stream> queimaRestante = getConnection().addStream(noDeManobra, "remainingBurnVector", noDeManobra.getReferenceFrame()); setCurrentStatus(Bundle.getString("status_maneuver_executing")); - double remainingBurnTime = duracaoDaQueima + 0.5; while (noDeManobra != null) { if (Thread.interrupted()) { throw new InterruptedException(); } - if (remainingBurnTime < 0.0 || queimaRestante.get().getValue1() < (fineAdjustment ? 2 : 0.5)) { + if (queimaRestante.get().getValue1() < (fineAdjustment ? 2 : 0.5)) { throttle(0.0f); break; } navigation.aimAtManeuver(noDeManobra); throttle(ctrlManeuver.calcPID((noDeManobra.getDeltaV() - Math.floor(queimaRestante.get().getValue1())) / noDeManobra.getDeltaV() * 1000, 1000)); - remainingBurnTime -= 0.05; Thread.sleep(50); } throttle(0.0f); diff --git a/src/com/pesterenan/views/ChangeVesselDialog.java b/src/com/pesterenan/views/ChangeVesselDialog.java index 3aacb24..527b9ab 100644 --- a/src/com/pesterenan/views/ChangeVesselDialog.java +++ b/src/com/pesterenan/views/ChangeVesselDialog.java @@ -31,7 +31,7 @@ public void initComponents() { // Buttons: btnChangeToVessel = new JButton("Mudar para"); - rbClosestVessels = new JRadioButton("Naves próximas (2km)"); + rbClosestVessels = new JRadioButton("Naves próximas (10km)"); rbOnSameBody = new JRadioButton("No mesmo corpo celeste"); rbAllVessels = new JRadioButton("Todas as naves"); diff --git a/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java b/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java index 3aa27b5..aadcb06 100644 --- a/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java +++ b/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java @@ -80,6 +80,7 @@ public void setupComponents() { btnCreateManeuver.setActionCommand(Modulos.MODULO_CRIAR_MANOBRAS.get()); btnCreateManeuver.setMaximumSize(btnFuncDimension); btnCreateManeuver.setPreferredSize(btnFuncDimension); + btnCreateManeuver.setEnabled(false); btnExecManeuver.addActionListener(this::changeFunctionPanel); btnExecManeuver.setActionCommand(Modulos.MODULO_EXEC_MANOBRAS.get()); btnExecManeuver.setMaximumSize(btnFuncDimension); diff --git a/src/com/pesterenan/views/LiftoffJPanel.java b/src/com/pesterenan/views/LiftoffJPanel.java index 8042a2b..fcaaad0 100644 --- a/src/com/pesterenan/views/LiftoffJPanel.java +++ b/src/com/pesterenan/views/LiftoffJPanel.java @@ -46,7 +46,7 @@ public void initComponents() { txfFinalApoapsis = new JTextField("80000"); txfFinalApoapsis.setToolTipText(Bundle.getString("pnl_lift_txf_final_apo_tooltip")); txfHeading = new JTextField("90"); - txfLimitTWR = new JTextField("1.5"); + txfLimitTWR = new JTextField("2.2"); // Buttons: btnLiftoff = new JButton(Bundle.getString("pnl_lift_btn_liftoff")); From 97b790c243b20a18d385c8b6aefd42b4291cc589 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Wed, 22 Feb 2023 00:37:18 -0300 Subject: [PATCH 03/19] [MP-2]: Making liftoff button get back to telemetry jpanel --- src/com/pesterenan/views/LiftoffJPanel.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/com/pesterenan/views/LiftoffJPanel.java b/src/com/pesterenan/views/LiftoffJPanel.java index fcaaad0..7c5e488 100644 --- a/src/com/pesterenan/views/LiftoffJPanel.java +++ b/src/com/pesterenan/views/LiftoffJPanel.java @@ -209,6 +209,7 @@ private void handleLiftoff(ActionEvent e) { commands.put(Modulos.USAR_ESTAGIOS.get(), String.valueOf(chkDecoupleStages.isSelected())); commands.put(Modulos.ABRIR_PAINEIS.get(), String.valueOf(chkOpenPanels.isSelected())); MechPeste.newInstance().startModule(commands); + MainGui.backToTelemetry(e); } } } From 76ccd006c48e1fea1efd44d8d17b3ba03e2642c7 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Wed, 22 Feb 2023 00:43:56 -0300 Subject: [PATCH 04/19] [MP-2]: Incremented patch version for bugfix --- src/com/pesterenan/views/AboutJFrame.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/com/pesterenan/views/AboutJFrame.java b/src/com/pesterenan/views/AboutJFrame.java index 7edc533..c3837e7 100644 --- a/src/com/pesterenan/views/AboutJFrame.java +++ b/src/com/pesterenan/views/AboutJFrame.java @@ -28,7 +28,7 @@ public AboutJFrame() { @Override public void initComponents() { // Labels: - lblMechpeste = new JLabel("MechPeste - v.0.6"); + lblMechpeste = new JLabel("MechPeste - v.0.6.1"); lblAboutInfo = new JLabel( "Esse app foi desenvolvido com o intuito de auxiliar o controle de naves
no game Kerbal Space Program.

" + "Não há garantias sobre o controle exato do app, portanto fique atento
" From b17afbdb124678f000a90bf5bd15787dcf57ae18 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Fri, 21 Apr 2023 01:02:08 -0300 Subject: [PATCH 05/19] [MP-2]: Created Panel and Functionality --- .../views/CreateManeuverJPanel.java | 147 ++++++++++++++++++ .../views/FunctionsAndTelemetryJPanel.java | 1 - src/com/pesterenan/views/MainGui.java | 5 +- 3 files changed, 150 insertions(+), 3 deletions(-) create mode 100644 src/com/pesterenan/views/CreateManeuverJPanel.java diff --git a/src/com/pesterenan/views/CreateManeuverJPanel.java b/src/com/pesterenan/views/CreateManeuverJPanel.java new file mode 100644 index 0000000..b244efe --- /dev/null +++ b/src/com/pesterenan/views/CreateManeuverJPanel.java @@ -0,0 +1,147 @@ +package com.pesterenan.views; + +import com.pesterenan.MechPeste; +import com.pesterenan.resources.Bundle; + +import krpc.client.RPCException; +import krpc.client.services.SpaceCenter.Node; +import krpc.client.services.SpaceCenter.Orbit; +import krpc.client.services.SpaceCenter.Vessel; +import krpc.client.services.SpaceCenter.VesselSituation; + +import javax.swing.*; +import javax.swing.border.TitledBorder; +import java.awt.*; +import java.awt.event.ActionEvent; +import java.awt.event.ActionListener; + +import static com.pesterenan.views.MainGui.PNL_DIMENSION; +import static com.pesterenan.views.MainGui.BTN_DIMENSION; +import static com.pesterenan.views.MainGui.MARGIN_BORDER_10_PX_LR; + +public class CreateManeuverJPanel extends JPanel implements ActionListener, UIMethods { + + private JLabel lblCreateManeuver, lblManeuverPosition; + private JButton btnCreateManeuver, btnApoapsis, btnPeriapsis, btnBack; + + public CreateManeuverJPanel() { + initComponents(); + setupComponents(); + layoutComponents(); + } + + @Override + public void initComponents() { + // Labels: + lblCreateManeuver = new JLabel("Criar plano de manobra:"); + lblManeuverPosition = new JLabel("Posicionar manobra no:"); + + // Buttons: + btnCreateManeuver = new JButton("Criar"); + btnApoapsis = new JButton("Apoastro"); + btnPeriapsis = new JButton("Periastro"); + btnBack = new JButton(Bundle.getString("pnl_mnv_btn_back")); + } + + @Override + public void setupComponents() { + // Main Panel setup: + setBorder(new TitledBorder(null, Bundle.getString("pnl_mnv_border"), TitledBorder.LEADING, TitledBorder.TOP, + null, null)); + + // Setting-up components: + btnCreateManeuver.addActionListener(this); + btnApoapsis.addActionListener(this); + btnPeriapsis.addActionListener(this); + btnBack.addActionListener(this); + btnBack.setMaximumSize(BTN_DIMENSION); + btnBack.setPreferredSize(BTN_DIMENSION); + } + + @Override + public void layoutComponents() { + // Main Panel layout: + setPreferredSize(PNL_DIMENSION); + setSize(PNL_DIMENSION); + setLayout(new BorderLayout()); + + JPanel pnlCreateManeuver = new JPanel(); + pnlCreateManeuver.setLayout(new BoxLayout(pnlCreateManeuver, BoxLayout.X_AXIS)); + pnlCreateManeuver.setBorder(MARGIN_BORDER_10_PX_LR); + pnlCreateManeuver.add(lblCreateManeuver); + pnlCreateManeuver.add(Box.createHorizontalGlue()); + pnlCreateManeuver.add(btnCreateManeuver); + + JPanel pnlPositionManeuver = new JPanel(); + pnlPositionManeuver.setLayout(new BoxLayout(pnlPositionManeuver, BoxLayout.X_AXIS)); + pnlPositionManeuver.setBorder(MARGIN_BORDER_10_PX_LR); + pnlPositionManeuver.add(lblManeuverPosition); + pnlPositionManeuver.add(Box.createHorizontalGlue()); + pnlPositionManeuver.add(btnApoapsis); + pnlPositionManeuver.add(Box.createHorizontalGlue()); + pnlPositionManeuver.add(btnPeriapsis); + + JPanel pnlButtons = new JPanel(); + pnlButtons.setLayout(new BoxLayout(pnlButtons, BoxLayout.X_AXIS)); + pnlButtons.add(Box.createHorizontalGlue()); + pnlButtons.add(btnBack); + + JPanel pnlMain = new JPanel(); + pnlMain.setLayout(new BoxLayout(pnlMain, BoxLayout.Y_AXIS)); + pnlMain.add(pnlCreateManeuver); + pnlMain.add(pnlPositionManeuver); + + add(pnlMain, BorderLayout.CENTER); + add(pnlButtons, BorderLayout.SOUTH); + } + + private void createManeuver() { + try { + MechPeste.newInstance(); + Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); + + if (vessel.getSituation() != VesselSituation.ORBITING) { + System.out.println("Não é possível criar a manobra fora de órbita."); + return; + } + double oneMinuteAhead = MechPeste.getSpaceCenter().getUT() + 60; + Node maneuverNode = vessel.getControl().addNode(oneMinuteAhead, 0, 0, 0); + // Print the maneuver node information + System.out.println("Maneuver Node created:"); + System.out.println(" Time to node: " + maneuverNode.getTimeTo() + " s"); + } catch (Exception e) { + } + } + + private void positionManeuverAt(Boolean apoastro) { + try { + MechPeste.newInstance(); + Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); + Orbit orbit = vessel.getOrbit(); + Node currentManeuver = vessel.getControl().getNodes().get(0); + double timeToNode = apoastro ? orbit.getTimeToApoapsis() : orbit.getTimeToPeriapsis(); + double maneuverTime = MechPeste.getSpaceCenter().getUT() + timeToNode; + currentManeuver.setUT(maneuverTime); + // Print the maneuver node information + System.out.println("Maneuver Node updated:"); + System.out.println(" Time to node: " + currentManeuver.getTimeTo() + " s"); + } catch (Exception e) { + } + } + + @Override + public void actionPerformed(ActionEvent e) { + if (e.getSource() == btnCreateManeuver) { + createManeuver(); + } + if (e.getSource() == btnApoapsis) { + positionManeuverAt(true); + } + if (e.getSource() == btnPeriapsis) { + positionManeuverAt(false); + } + if (e.getSource() == btnBack) { + MainGui.backToTelemetry(e); + } + } +} diff --git a/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java b/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java index aadcb06..3aa27b5 100644 --- a/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java +++ b/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java @@ -80,7 +80,6 @@ public void setupComponents() { btnCreateManeuver.setActionCommand(Modulos.MODULO_CRIAR_MANOBRAS.get()); btnCreateManeuver.setMaximumSize(btnFuncDimension); btnCreateManeuver.setPreferredSize(btnFuncDimension); - btnCreateManeuver.setEnabled(false); btnExecManeuver.addActionListener(this::changeFunctionPanel); btnExecManeuver.setActionCommand(Modulos.MODULO_EXEC_MANOBRAS.get()); btnExecManeuver.setMaximumSize(btnFuncDimension); diff --git a/src/com/pesterenan/views/MainGui.java b/src/com/pesterenan/views/MainGui.java index 04c903a..aedb2ee 100644 --- a/src/com/pesterenan/views/MainGui.java +++ b/src/com/pesterenan/views/MainGui.java @@ -9,8 +9,6 @@ import java.awt.*; import java.awt.event.ActionEvent; import java.awt.event.ActionListener; -import java.beans.PropertyChangeEvent; -import java.beans.PropertyChangeListener; public class MainGui extends JFrame implements ActionListener, UIMethods { @@ -32,6 +30,7 @@ public class MainGui extends JFrame implements ActionListener, UIMethods { private final static CardLayout cardLayout = new CardLayout(0, 0); private LiftoffJPanel pnlLiftoff; private LandingJPanel pnlLanding; + private CreateManeuverJPanel pnlCreateManeuver; private ManeuverJPanel pnlManeuver; private RoverJPanel pnlRover; @@ -73,6 +72,7 @@ public void initComponents() { pnlFunctionsAndTelemetry = new FunctionsAndTelemetryJPanel(); pnlLiftoff = new LiftoffJPanel(); pnlLanding = new LandingJPanel(); + pnlCreateManeuver = new CreateManeuverJPanel(); pnlManeuver = new ManeuverJPanel(); pnlRover = new RoverJPanel(); pnlStatus = new StatusJPanel(); @@ -124,6 +124,7 @@ public void layoutComponents() { cardJPanels.add(pnlFunctionsAndTelemetry, Modulos.MODULO_TELEMETRIA.get()); cardJPanels.add(pnlLiftoff, Modulos.MODULO_DECOLAGEM.get()); cardJPanels.add(pnlLanding, Modulos.MODULO_POUSO.get()); + cardJPanels.add(pnlCreateManeuver, Modulos.MODULO_CRIAR_MANOBRAS.get()); cardJPanels.add(pnlManeuver, Modulos.MODULO_EXEC_MANOBRAS.get()); cardJPanels.add(pnlRover, Modulos.MODULO_ROVER.get()); } From 7f8f4a8c9b183d1c8b217f226e4d6bacfc26ae95 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Mon, 1 May 2023 23:15:07 -0300 Subject: [PATCH 06/19] [MP-2]: Added event listeners for maneuver buttons --- src/com/pesterenan/MechPeste.java | 19 + .../views/CreateManeuverJPanel.java | 327 +++++++++++++++--- 2 files changed, 307 insertions(+), 39 deletions(-) diff --git a/src/com/pesterenan/MechPeste.java b/src/com/pesterenan/MechPeste.java index 4f6fc12..6e72fc6 100644 --- a/src/com/pesterenan/MechPeste.java +++ b/src/com/pesterenan/MechPeste.java @@ -3,12 +3,14 @@ import com.pesterenan.model.ActiveVessel; import com.pesterenan.resources.Bundle; import com.pesterenan.utils.Vector; +import com.pesterenan.views.CreateManeuverJPanel; import com.pesterenan.views.FunctionsAndTelemetryJPanel; import com.pesterenan.views.MainGui; import krpc.client.Connection; import krpc.client.RPCException; import krpc.client.services.KRPC; import krpc.client.services.SpaceCenter; +import krpc.client.services.SpaceCenter.Node; import krpc.client.services.SpaceCenter.Vessel; import javax.swing.*; @@ -70,6 +72,22 @@ public static ListModel getActiveVessels(String search) { return list; } + public static ListModel getCurrentManeuvers() { + DefaultListModel list = new DefaultListModel<>(); + try { + List maneuvers = getSpaceCenter().getActiveVessel().getControl().getNodes(); + maneuvers.forEach(m -> { + try { + String maneuverStr = String.format("%d - \tDv: %.1f", maneuvers.indexOf(m) + 1, m.getDeltaV()); + list.addElement(maneuverStr); + } catch (RPCException ignored) { + } + }); + } catch (RPCException | NullPointerException ignored) { + } + return list; + } + private static boolean filterVessels(Vessel vessel, String search) { if (search == "all") { return true; @@ -144,6 +162,7 @@ private void checkActiveVessel() { setStatusMessage(currentVessel.getCurrentStatus()); } FunctionsAndTelemetryJPanel.updateTelemetry(currentVessel.getTelemetryData()); + CreateManeuverJPanel.updateManeuverList(getCurrentManeuvers()); } Thread.sleep(100); } catch (RPCException | InterruptedException ignored) { diff --git a/src/com/pesterenan/views/CreateManeuverJPanel.java b/src/com/pesterenan/views/CreateManeuverJPanel.java index b244efe..2c00d7b 100644 --- a/src/com/pesterenan/views/CreateManeuverJPanel.java +++ b/src/com/pesterenan/views/CreateManeuverJPanel.java @@ -11,18 +11,29 @@ import javax.swing.*; import javax.swing.border.TitledBorder; + +import org.javatuples.Pair; + import java.awt.*; import java.awt.event.ActionEvent; import java.awt.event.ActionListener; +import java.util.HashMap; +import java.util.Hashtable; +import java.util.Map; import static com.pesterenan.views.MainGui.PNL_DIMENSION; import static com.pesterenan.views.MainGui.BTN_DIMENSION; -import static com.pesterenan.views.MainGui.MARGIN_BORDER_10_PX_LR; public class CreateManeuverJPanel extends JPanel implements ActionListener, UIMethods { - private JLabel lblCreateManeuver, lblManeuverPosition; - private JButton btnCreateManeuver, btnApoapsis, btnPeriapsis, btnBack; + private static JButton btnCreateManeuver, btnDeleteManeuver, btnBack, btnAp, btnPe, btnAN, btnDN; + private static JButton btnIncrease, btnDecrease, btnNextOrbit, btnPrevOrbit; + private static JSlider sldScale; + private static JList listCurrentManeuvers; + private static int selectedManeuverIndex = 0; + private static JRadioButton rbPrograde, rbNormal, rbRadial, rbTime; + private static ButtonGroup bgManeuverType; + private static Map> sliderValues = new HashMap<>(); public CreateManeuverJPanel() { initComponents(); @@ -32,15 +43,35 @@ public CreateManeuverJPanel() { @Override public void initComponents() { - // Labels: - lblCreateManeuver = new JLabel("Criar plano de manobra:"); - lblManeuverPosition = new JLabel("Posicionar manobra no:"); - // Buttons: btnCreateManeuver = new JButton("Criar"); - btnApoapsis = new JButton("Apoastro"); - btnPeriapsis = new JButton("Periastro"); + btnDeleteManeuver = new JButton("Apagar"); + btnAp = new JButton("AP"); + btnPe = new JButton("PE"); + btnAN = new JButton("NA"); + btnDN = new JButton("ND"); btnBack = new JButton(Bundle.getString("pnl_mnv_btn_back")); + btnIncrease = new JButton("+"); + btnDecrease = new JButton("-"); + btnNextOrbit = new JButton(">"); + btnPrevOrbit = new JButton("<"); + + // Radio buttons: + rbPrograde = new JRadioButton("Pro"); + rbNormal = new JRadioButton("Nor"); + rbRadial = new JRadioButton("Rad"); + rbTime = new JRadioButton("Tmp"); + + // Misc: + listCurrentManeuvers = new JList<>(); + sldScale = new JSlider(JSlider.VERTICAL, 0, 5, 2); + bgManeuverType = new ButtonGroup(); + sliderValues.put(0, new Pair<>("0.01m/s", 0.01f)); + sliderValues.put(1, new Pair<>("0.10m/s", 0.1f)); + sliderValues.put(2, new Pair<>("1.0 m/s", 1f)); + sliderValues.put(3, new Pair<>("10 m/s", 10f)); + sliderValues.put(4, new Pair<>("100 m/s", 100f)); + sliderValues.put(5, new Pair<>("1k m/s", 1000f)); } @Override @@ -51,11 +82,94 @@ public void setupComponents() { // Setting-up components: btnCreateManeuver.addActionListener(this); - btnApoapsis.addActionListener(this); - btnPeriapsis.addActionListener(this); + btnCreateManeuver.setMaximumSize(BTN_DIMENSION); + btnCreateManeuver.setPreferredSize(BTN_DIMENSION); + btnDeleteManeuver.addActionListener(this); + btnDeleteManeuver.setMaximumSize(BTN_DIMENSION); + btnDeleteManeuver.setPreferredSize(BTN_DIMENSION); + btnAp.addActionListener(this); + btnAp.setActionCommand("apoapsis"); + btnPe.addActionListener(this); + btnPe.setActionCommand("periapsis"); + btnAN.addActionListener(this); + btnAN.setActionCommand("ascending"); + btnDN.addActionListener(this); + btnDN.setActionCommand("descending"); btnBack.addActionListener(this); btnBack.setMaximumSize(BTN_DIMENSION); btnBack.setPreferredSize(BTN_DIMENSION); + btnIncrease.setActionCommand("increase"); + btnDecrease.setActionCommand("decrease"); + btnNextOrbit.setActionCommand("next_orbit"); + btnPrevOrbit.setActionCommand("prev_orbit"); + btnIncrease.addActionListener(this); + btnIncrease.addMouseWheelListener(e -> { + int rotation = e.getWheelRotation(); + if (rotation > 0) { + changeManeuverDeltaV(btnDecrease.getActionCommand()); + } else { + changeManeuverDeltaV(btnIncrease.getActionCommand()); + } + }); + btnDecrease.addActionListener(this); + btnDecrease.addMouseWheelListener(e -> { + int rotation = e.getWheelRotation(); + if (rotation > 0) { + changeManeuverDeltaV(btnIncrease.getActionCommand()); + } else { + changeManeuverDeltaV(btnDecrease.getActionCommand()); + } + });; + btnNextOrbit.addActionListener(this); + btnPrevOrbit.addActionListener(this); + btnIncrease.setMaximumSize(new Dimension(70, 26)); + btnDecrease.setMaximumSize(new Dimension(70, 26)); + btnNextOrbit.setMaximumSize(new Dimension(35, 26)); + btnPrevOrbit.setMaximumSize(new Dimension(35, 26)); + btnIncrease.setPreferredSize(new Dimension(70, 26)); + btnDecrease.setPreferredSize(new Dimension(70, 26)); + btnNextOrbit.setPreferredSize(new Dimension(35, 26)); + btnPrevOrbit.setPreferredSize(new Dimension(35, 26)); + btnIncrease.setMargin(new Insets(0, 0, 0, 0)); + btnDecrease.setMargin(new Insets(0, 0, 0, 0)); + btnNextOrbit.setMargin(new Insets(0, 0, 0, 0)); + btnPrevOrbit.setMargin(new Insets(0, 0, 0, 0)); + + rbPrograde.setActionCommand("prograde"); + rbNormal.setActionCommand("normal"); + rbRadial.setActionCommand("radial"); + rbTime.setActionCommand("time"); + rbPrograde.setSelected(true); + bgManeuverType.add(rbPrograde); + bgManeuverType.add(rbNormal); + bgManeuverType.add(rbRadial); + bgManeuverType.add(rbTime); + + listCurrentManeuvers.setSelectionMode(ListSelectionModel.SINGLE_SELECTION); + listCurrentManeuvers.addListSelectionListener(e -> selectedManeuverIndex = e.getFirstIndex()); + + sldScale.setSnapToTicks(true); + sldScale.setPaintTicks(true); + sldScale.setMajorTickSpacing(1); + sldScale.setMinorTickSpacing(1); + Hashtable labelTable = new Hashtable(); + labelTable.put(0, new JLabel(sliderValues.get(0).getValue0())); + labelTable.put(1, new JLabel(sliderValues.get(1).getValue0())); + labelTable.put(2, new JLabel(sliderValues.get(2).getValue0())); + labelTable.put(3, new JLabel(sliderValues.get(3).getValue0())); + labelTable.put(4, new JLabel(sliderValues.get(4).getValue0())); + labelTable.put(5, new JLabel(sliderValues.get(5).getValue0())); + sldScale.setLabelTable(labelTable); + sldScale.setPaintLabels(true); + sldScale.addChangeListener(e -> handleChangeButtonText(sldScale.getValue())); + sldScale.addMouseWheelListener(e -> { + int rotation = e.getWheelRotation(); + if (rotation < 0) { + sldScale.setValue(sldScale.getValue() + sldScale.getMinorTickSpacing()); + } else { + sldScale.setValue(sldScale.getValue() - sldScale.getMinorTickSpacing()); + } + }); } @Override @@ -65,34 +179,108 @@ public void layoutComponents() { setSize(PNL_DIMENSION); setLayout(new BorderLayout()); - JPanel pnlCreateManeuver = new JPanel(); - pnlCreateManeuver.setLayout(new BoxLayout(pnlCreateManeuver, BoxLayout.X_AXIS)); - pnlCreateManeuver.setBorder(MARGIN_BORDER_10_PX_LR); - pnlCreateManeuver.add(lblCreateManeuver); - pnlCreateManeuver.add(Box.createHorizontalGlue()); - pnlCreateManeuver.add(btnCreateManeuver); - JPanel pnlPositionManeuver = new JPanel(); pnlPositionManeuver.setLayout(new BoxLayout(pnlPositionManeuver, BoxLayout.X_AXIS)); - pnlPositionManeuver.setBorder(MARGIN_BORDER_10_PX_LR); - pnlPositionManeuver.add(lblManeuverPosition); + pnlPositionManeuver.setBorder(new TitledBorder("Posicionar manobra no:")); pnlPositionManeuver.add(Box.createHorizontalGlue()); - pnlPositionManeuver.add(btnApoapsis); + pnlPositionManeuver.add(btnAp); + pnlPositionManeuver.add(btnPe); + pnlPositionManeuver.add(btnAN); + pnlPositionManeuver.add(btnDN); pnlPositionManeuver.add(Box.createHorizontalGlue()); - pnlPositionManeuver.add(btnPeriapsis); - JPanel pnlButtons = new JPanel(); - pnlButtons.setLayout(new BoxLayout(pnlButtons, BoxLayout.X_AXIS)); - pnlButtons.add(Box.createHorizontalGlue()); - pnlButtons.add(btnBack); + JPanel pnlRadioButtons = new JPanel(new GridLayout(4, 1)); + pnlRadioButtons.setMaximumSize(new Dimension(50, 100)); + pnlRadioButtons.add(rbPrograde); + pnlRadioButtons.add(rbNormal); + pnlRadioButtons.add(rbRadial); + pnlRadioButtons.add(rbTime); + + JPanel pnlSlider = new JPanel(); + pnlSlider.setLayout(new BoxLayout(pnlSlider, BoxLayout.Y_AXIS)); + pnlSlider.setAlignmentX(Component.LEFT_ALIGNMENT); + pnlSlider.setBorder(new TitledBorder("Escala:")); + pnlSlider.setPreferredSize(new Dimension(50, 100)); + pnlSlider.add(sldScale); + + JPanel pnlOrbitControl = new JPanel(); + pnlOrbitControl.setLayout(new BoxLayout(pnlOrbitControl, BoxLayout.X_AXIS)); + pnlOrbitControl.setAlignmentX(Component.LEFT_ALIGNMENT); + pnlOrbitControl.add(btnPrevOrbit); + pnlOrbitControl.add(btnNextOrbit); + + JPanel pnlManeuverButtons = new JPanel(); + pnlManeuverButtons.setLayout(new BoxLayout(pnlManeuverButtons, BoxLayout.Y_AXIS)); + pnlManeuverButtons.setAlignmentX(Component.LEFT_ALIGNMENT); + pnlManeuverButtons.add(btnIncrease); + pnlManeuverButtons.add(pnlOrbitControl); + pnlManeuverButtons.add(btnDecrease); + + JPanel pnlManeuverController = new JPanel(); + pnlManeuverController.setLayout(new BoxLayout(pnlManeuverController, BoxLayout.X_AXIS)); + pnlManeuverController.setBorder(new TitledBorder("Controlador de Manobra:")); + pnlManeuverController.setMaximumSize(new Dimension(400, 300)); + pnlManeuverController.add(pnlRadioButtons); + pnlManeuverController.add(pnlSlider); + pnlManeuverController.add(pnlManeuverButtons); + + JPanel pnlControls = new JPanel(); + pnlControls.setLayout(new BoxLayout(pnlControls, BoxLayout.Y_AXIS)); + pnlControls.add(pnlPositionManeuver); + pnlControls.add(pnlManeuverController); + + JPanel pnlManeuverList = new JPanel(); + pnlManeuverList.setLayout(new BoxLayout(pnlManeuverList, BoxLayout.Y_AXIS)); + JScrollPane scrollPane = new JScrollPane(); + scrollPane.setViewportView(listCurrentManeuvers); + pnlManeuverList.add(scrollPane); + pnlManeuverList.add(btnCreateManeuver); + pnlManeuverList.add(btnDeleteManeuver); + + JPanel pnlOptions = new JPanel(); + pnlOptions.setLayout(new BoxLayout(pnlOptions, BoxLayout.Y_AXIS)); + pnlOptions.setBorder(new TitledBorder("Lista de Manobras:")); + pnlOptions.add(pnlManeuverList); + pnlOptions.setPreferredSize(new Dimension(110, 300)); + pnlOptions.setMaximumSize(new Dimension(110, 300)); + + JPanel pnlOptionsAndList = new JPanel(); + pnlOptionsAndList.setLayout(new BoxLayout(pnlOptionsAndList, BoxLayout.X_AXIS)); + pnlControls.setAlignmentY(TOP_ALIGNMENT); + pnlOptions.setAlignmentY(TOP_ALIGNMENT); + pnlOptionsAndList.add(pnlOptions); + pnlOptionsAndList.add(Box.createHorizontalStrut(5)); + pnlOptionsAndList.add(pnlControls); + + JPanel pnlBackButton = new JPanel(); + pnlBackButton.setLayout(new BoxLayout(pnlBackButton, BoxLayout.X_AXIS)); + pnlBackButton.add(Box.createHorizontalGlue()); + pnlBackButton.add(btnBack); JPanel pnlMain = new JPanel(); - pnlMain.setLayout(new BoxLayout(pnlMain, BoxLayout.Y_AXIS)); - pnlMain.add(pnlCreateManeuver); - pnlMain.add(pnlPositionManeuver); + pnlMain.setLayout(new BoxLayout(pnlMain, BoxLayout.X_AXIS)); + pnlMain.add(pnlOptionsAndList); add(pnlMain, BorderLayout.CENTER); - add(pnlButtons, BorderLayout.SOUTH); + add(pnlBackButton, BorderLayout.SOUTH); + } + + public static void updateManeuverList(ListModel list) { + listCurrentManeuvers.setModel(list); + listCurrentManeuvers.setSelectedIndex(selectedManeuverIndex); + btnDeleteManeuver.setEnabled(list.getSize() > 0); + try { + btnAp.setEnabled(list.getSize() > 0); + btnPe.setEnabled(list.getSize() > 0); + btnAN.setEnabled(list.getSize() > 0 && MechPeste.getSpaceCenter().getTargetVessel() != null); + btnDN.setEnabled(list.getSize() > 0 && MechPeste.getSpaceCenter().getTargetVessel() != null); + } catch (RPCException ignored) { + } + } + + private static void handleChangeButtonText(int value) { + btnIncrease.setText("+ " + sliderValues.get(value).getValue0()); + btnDecrease.setText("- " + sliderValues.get(value).getValue0()); } private void createManeuver() { @@ -113,15 +301,42 @@ private void createManeuver() { } } - private void positionManeuverAt(Boolean apoastro) { + private void deleteManeuver() { + try { + MechPeste.newInstance(); + Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); + Node currentManeuver = vessel.getControl().getNodes().get(selectedManeuverIndex); + currentManeuver.remove(); + } catch (Exception e) { + } + } + + private void positionManeuverAt(String node) { try { MechPeste.newInstance(); Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); Orbit orbit = vessel.getOrbit(); - Node currentManeuver = vessel.getControl().getNodes().get(0); - double timeToNode = apoastro ? orbit.getTimeToApoapsis() : orbit.getTimeToPeriapsis(); - double maneuverTime = MechPeste.getSpaceCenter().getUT() + timeToNode; - currentManeuver.setUT(maneuverTime); + Node currentManeuver = vessel.getControl().getNodes().get(selectedManeuverIndex); + double timeToNode = 0; + switch (node) { + case "apoapsis": + timeToNode = MechPeste.getSpaceCenter().getUT() + orbit.getTimeToApoapsis(); + break; + case "periapsis": + timeToNode = MechPeste.getSpaceCenter().getUT() + orbit.getTimeToPeriapsis(); + break; + case "ascending": + double ascendingAnomaly = orbit + .trueAnomalyAtAN(MechPeste.getSpaceCenter().getTargetVessel().getOrbit()); + timeToNode = orbit.uTAtTrueAnomaly(ascendingAnomaly); + break; + case "descending": + double descendingAnomaly = orbit + .trueAnomalyAtDN(MechPeste.getSpaceCenter().getTargetVessel().getOrbit()); + timeToNode = orbit.uTAtTrueAnomaly(descendingAnomaly); + break; + } + currentManeuver.setUT(timeToNode); // Print the maneuver node information System.out.println("Maneuver Node updated:"); System.out.println(" Time to node: " + currentManeuver.getTimeTo() + " s"); @@ -129,16 +344,50 @@ private void positionManeuverAt(Boolean apoastro) { } } + private void changeManeuverDeltaV(String command) { + try { + MechPeste.newInstance(); + Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); + Node currentManeuver = vessel.getControl().getNodes().get(selectedManeuverIndex); + String maneuverType = bgManeuverType.getSelection().getActionCommand(); + float currentSliderValue = sliderValues.get(sldScale.getValue()).getValue1(); + currentSliderValue = command == "decrease" ? -currentSliderValue : currentSliderValue; + + switch (maneuverType) { + case "prograde": + currentManeuver.setPrograde(currentManeuver.getPrograde() + currentSliderValue); + break; + case "normal": + currentManeuver.setNormal(currentManeuver.getNormal() + currentSliderValue); + break; + case "radial": + currentManeuver.setRadial(currentManeuver.getRadial() + currentSliderValue); + break; + case "time": + currentManeuver.setUT(currentManeuver.getUT() + currentSliderValue); + break; + } + } catch (RPCException e) { + System.err.println("Erro RPC ao mudar o delta-V da manobra: " + e.getMessage()); + } catch (Exception e) { + System.err.println("Erro ao mudar o delta-V da manobra: " + e.getMessage()); + } + + } + @Override public void actionPerformed(ActionEvent e) { if (e.getSource() == btnCreateManeuver) { createManeuver(); } - if (e.getSource() == btnApoapsis) { - positionManeuverAt(true); + if (e.getSource() == btnDeleteManeuver) { + deleteManeuver(); + } + if (e.getSource() == btnIncrease || e.getSource() == btnDecrease) { + changeManeuverDeltaV(e.getActionCommand()); } - if (e.getSource() == btnPeriapsis) { - positionManeuverAt(false); + if (e.getSource() == btnAp || e.getSource() == btnPe || e.getSource() == btnAN || e.getSource() == btnDN) { + positionManeuverAt(e.getActionCommand()); } if (e.getSource() == btnBack) { MainGui.backToTelemetry(e); From cc7ff55b71d5ae2edc9ad2d156d0c94a06f557e7 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Wed, 3 May 2023 00:00:20 -0300 Subject: [PATCH 07/19] [MP-2]: Began implementing Rendezvous code --- .../views/CreateManeuverJPanel.java | 95 +++++++++++++++---- 1 file changed, 79 insertions(+), 16 deletions(-) diff --git a/src/com/pesterenan/views/CreateManeuverJPanel.java b/src/com/pesterenan/views/CreateManeuverJPanel.java index 2c00d7b..905a98f 100644 --- a/src/com/pesterenan/views/CreateManeuverJPanel.java +++ b/src/com/pesterenan/views/CreateManeuverJPanel.java @@ -19,6 +19,7 @@ import java.awt.event.ActionListener; import java.util.HashMap; import java.util.Hashtable; +import java.util.List; import java.util.Map; import static com.pesterenan.views.MainGui.PNL_DIMENSION; @@ -27,7 +28,7 @@ public class CreateManeuverJPanel extends JPanel implements ActionListener, UIMethods { private static JButton btnCreateManeuver, btnDeleteManeuver, btnBack, btnAp, btnPe, btnAN, btnDN; - private static JButton btnIncrease, btnDecrease, btnNextOrbit, btnPrevOrbit; + private static JButton btnIncrease, btnDecrease, btnNextOrbit, btnPrevOrbit, btnAlignPlanes, btnRendezvous; private static JSlider sldScale; private static JList listCurrentManeuvers; private static int selectedManeuverIndex = 0; @@ -55,6 +56,8 @@ public void initComponents() { btnDecrease = new JButton("-"); btnNextOrbit = new JButton(">"); btnPrevOrbit = new JButton("<"); + btnAlignPlanes = new JButton("Alinhar planos"); + btnRendezvous = new JButton("Rendezvous"); // Radio buttons: rbPrograde = new JRadioButton("Pro"); @@ -99,9 +102,6 @@ public void setupComponents() { btnBack.setMaximumSize(BTN_DIMENSION); btnBack.setPreferredSize(BTN_DIMENSION); btnIncrease.setActionCommand("increase"); - btnDecrease.setActionCommand("decrease"); - btnNextOrbit.setActionCommand("next_orbit"); - btnPrevOrbit.setActionCommand("prev_orbit"); btnIncrease.addActionListener(this); btnIncrease.addMouseWheelListener(e -> { int rotation = e.getWheelRotation(); @@ -111,6 +111,10 @@ public void setupComponents() { changeManeuverDeltaV(btnIncrease.getActionCommand()); } }); + btnIncrease.setMaximumSize(new Dimension(70, 26)); + btnIncrease.setPreferredSize(new Dimension(70, 26)); + btnIncrease.setMargin(new Insets(0, 0, 0, 0)); + btnDecrease.setActionCommand("decrease"); btnDecrease.addActionListener(this); btnDecrease.addMouseWheelListener(e -> { int rotation = e.getWheelRotation(); @@ -119,21 +123,47 @@ public void setupComponents() { } else { changeManeuverDeltaV(btnDecrease.getActionCommand()); } - });; - btnNextOrbit.addActionListener(this); - btnPrevOrbit.addActionListener(this); - btnIncrease.setMaximumSize(new Dimension(70, 26)); + }); btnDecrease.setMaximumSize(new Dimension(70, 26)); - btnNextOrbit.setMaximumSize(new Dimension(35, 26)); - btnPrevOrbit.setMaximumSize(new Dimension(35, 26)); - btnIncrease.setPreferredSize(new Dimension(70, 26)); btnDecrease.setPreferredSize(new Dimension(70, 26)); - btnNextOrbit.setPreferredSize(new Dimension(35, 26)); - btnPrevOrbit.setPreferredSize(new Dimension(35, 26)); - btnIncrease.setMargin(new Insets(0, 0, 0, 0)); btnDecrease.setMargin(new Insets(0, 0, 0, 0)); + btnNextOrbit.setActionCommand("next_orbit"); + btnNextOrbit.addActionListener(this); + btnNextOrbit.setMaximumSize(new Dimension(35, 26)); + btnNextOrbit.setPreferredSize(new Dimension(35, 26)); btnNextOrbit.setMargin(new Insets(0, 0, 0, 0)); + btnPrevOrbit.setActionCommand("prev_orbit"); + btnPrevOrbit.addActionListener(this); + btnPrevOrbit.setMaximumSize(new Dimension(35, 26)); + btnPrevOrbit.setPreferredSize(new Dimension(35, 26)); btnPrevOrbit.setMargin(new Insets(0, 0, 0, 0)); + btnAlignPlanes.addActionListener(e -> { + try { + MechPeste.newInstance(); + Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); + Vessel targetVessel = MechPeste.getSpaceCenter().getTargetVessel(); + List currentManeuvers = vessel.getControl().getNodes(); + if (currentManeuvers.size() < 1) { + createManeuver(); + positionManeuverAt("ascending"); + } + Node currentManeuver = currentManeuvers.get(0); + double currentInclination = currentManeuver.getOrbit().relativeInclination(targetVessel.getOrbit()); + System.out.println(Math.toDegrees(currentInclination)); + } catch (Exception err) { + } + }); + btnRendezvous.addActionListener(e -> { + try { + MechPeste.newInstance(); + Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); + Vessel targetVessel = MechPeste.getSpaceCenter().getTargetVessel(); + Node currentManeuver = vessel.getControl().getNodes().get(selectedManeuverIndex); + double closestApproach = currentManeuver.getOrbit().distanceAtClosestApproach(targetVessel.getOrbit()); + System.out.println(closestApproach); + } catch (Exception err) { + } + }); rbPrograde.setActionCommand("prograde"); rbNormal.setActionCommand("normal"); @@ -219,15 +249,27 @@ public void layoutComponents() { JPanel pnlManeuverController = new JPanel(); pnlManeuverController.setLayout(new BoxLayout(pnlManeuverController, BoxLayout.X_AXIS)); pnlManeuverController.setBorder(new TitledBorder("Controlador de Manobra:")); - pnlManeuverController.setMaximumSize(new Dimension(400, 300)); + pnlManeuverController.setMaximumSize(new Dimension(380, 300)); pnlManeuverController.add(pnlRadioButtons); pnlManeuverController.add(pnlSlider); pnlManeuverController.add(pnlManeuverButtons); + + JPanel pnlAutoPosition = new JPanel(); + pnlAutoPosition.setLayout(new BoxLayout(pnlAutoPosition, BoxLayout.Y_AXIS)); + pnlAutoPosition.setBorder(new TitledBorder("Auto posição:")); + pnlAutoPosition.setMaximumSize(new Dimension(400, 300)); + pnlAutoPosition.add(btnAlignPlanes); + pnlAutoPosition.add(btnRendezvous); + + JPanel pnlMCpnlAP = new JPanel(); + pnlMCpnlAP.setLayout(new BoxLayout(pnlMCpnlAP, BoxLayout.X_AXIS)); + pnlMCpnlAP.add(pnlManeuverController); + pnlMCpnlAP.add(pnlAutoPosition); JPanel pnlControls = new JPanel(); pnlControls.setLayout(new BoxLayout(pnlControls, BoxLayout.Y_AXIS)); pnlControls.add(pnlPositionManeuver); - pnlControls.add(pnlManeuverController); + pnlControls.add(pnlMCpnlAP); JPanel pnlManeuverList = new JPanel(); pnlManeuverList.setLayout(new BoxLayout(pnlManeuverList, BoxLayout.Y_AXIS)); @@ -375,6 +417,24 @@ private void changeManeuverDeltaV(String command) { } + private void changeOrbit(String command) { + try { + MechPeste.newInstance(); + Vessel vessel; + vessel = MechPeste.getSpaceCenter().getActiveVessel(); + Node currentManeuver = vessel.getControl().getNodes().get(selectedManeuverIndex); + double currentOrbitPeriod = vessel.getOrbit().getPeriod(); + if (command == "next_orbit") { + currentManeuver.setUT(currentManeuver.getUT() + currentOrbitPeriod); + } else { + double newUT = currentManeuver.getUT() - currentOrbitPeriod; + newUT = newUT < MechPeste.getSpaceCenter().getUT() ? MechPeste.getSpaceCenter().getUT() + 60 : newUT; + currentManeuver.setUT(newUT); + } + } catch (RPCException ignored) { + } + } + @Override public void actionPerformed(ActionEvent e) { if (e.getSource() == btnCreateManeuver) { @@ -386,6 +446,9 @@ public void actionPerformed(ActionEvent e) { if (e.getSource() == btnIncrease || e.getSource() == btnDecrease) { changeManeuverDeltaV(e.getActionCommand()); } + if (e.getSource() == btnNextOrbit || e.getSource() == btnPrevOrbit) { + changeOrbit(e.getActionCommand()); + } if (e.getSource() == btnAp || e.getSource() == btnPe || e.getSource() == btnAN || e.getSource() == btnDN) { positionManeuverAt(e.getActionCommand()); } From 24355e6ef316bc3d2150f1828b6be9350b1bedb3 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Tue, 9 May 2023 23:23:48 -0300 Subject: [PATCH 08/19] [MP-2]: Unified Create and Run Maneuver JPanels --- src/com/pesterenan/MechPeste.java | 24 ++-- .../controllers/LiftoffController.java | 2 +- src/com/pesterenan/model/ActiveVessel.java | 2 +- .../resources/MechPesteBundle.properties | 2 +- .../resources/MechPesteBundle_pt.properties | 2 +- src/com/pesterenan/utils/Modulos.java | 2 +- .../views/CreateManeuverJPanel.java | 136 +++++++++++++----- .../views/FunctionsAndTelemetryJPanel.java | 21 +-- src/com/pesterenan/views/MainGui.java | 31 ++-- ...uverJPanel.java => RunManeuverJPanel.java} | 10 +- 10 files changed, 144 insertions(+), 88 deletions(-) rename src/com/pesterenan/views/{ManeuverJPanel.java => RunManeuverJPanel.java} (94%) diff --git a/src/com/pesterenan/MechPeste.java b/src/com/pesterenan/MechPeste.java index 6e72fc6..ee247fa 100644 --- a/src/com/pesterenan/MechPeste.java +++ b/src/com/pesterenan/MechPeste.java @@ -78,7 +78,9 @@ public static ListModel getCurrentManeuvers() { List maneuvers = getSpaceCenter().getActiveVessel().getControl().getNodes(); maneuvers.forEach(m -> { try { - String maneuverStr = String.format("%d - \tDv: %.1f", maneuvers.indexOf(m) + 1, m.getDeltaV()); + String maneuverStr = String.format("%d - Dv: %.1f {P: %.1f, N: %.1f, R: %.1f} AP: %.1f, PE: %.1f", + maneuvers.indexOf(m) + 1, m.getDeltaV(), m.getPrograde(), m.getNormal(), m.getRadial(), + m.getOrbit().getApoapsisAltitude(), m.getOrbit().getPeriapsisAltitude()); list.addElement(maneuverStr); } catch (RPCException ignored) { } @@ -116,13 +118,13 @@ private static boolean filterVessels(Vessel vessel, String search) { public static String getVesselInfo(int selectedIndex) { try { - Vessel naveAtual = - spaceCenter.getVessels().stream().filter(v -> v.hashCode() == selectedIndex).findFirst().get(); + Vessel naveAtual = spaceCenter.getVessels().stream().filter(v -> v.hashCode() == selectedIndex).findFirst() + .get(); String name = naveAtual.getName().length() > 40 - ? naveAtual.getName().substring(0, 40) + "..." - : naveAtual.getName(); - String vesselInfo = - String.format("Nome: %s\t\t\t | Corpo: %s", name, naveAtual.getOrbit().getBody().getName()); + ? naveAtual.getName().substring(0, 40) + "..." + : naveAtual.getName(); + String vesselInfo = String.format("Nome: %s\t\t\t | Corpo: %s", name, + naveAtual.getOrbit().getBody().getName()); return vesselInfo; } catch (RPCException | NullPointerException ignored) { } @@ -131,8 +133,8 @@ public static String getVesselInfo(int selectedIndex) { public static void changeToVessel(int selectedIndex) { try { - Vessel naveAtual = - spaceCenter.getVessels().stream().filter(v -> v.hashCode() == selectedIndex).findFirst().get(); + Vessel naveAtual = spaceCenter.getVessels().stream().filter(v -> v.hashCode() == selectedIndex).findFirst() + .get(); spaceCenter.setActiveVessel(naveAtual); } catch (RPCException | NullPointerException e) { System.out.println(Bundle.getString("status_couldnt_switch_vessel")); @@ -158,11 +160,11 @@ private void checkActiveVessel() { } if (currentVesselId != -1) { currentVessel.recordTelemetryData(); - if (currentVessel.hasModuleRunning()){ + if (currentVessel.hasModuleRunning()) { setStatusMessage(currentVessel.getCurrentStatus()); } FunctionsAndTelemetryJPanel.updateTelemetry(currentVessel.getTelemetryData()); - CreateManeuverJPanel.updateManeuverList(getCurrentManeuvers()); + CreateManeuverJPanel.updatePanel(getCurrentManeuvers()); } Thread.sleep(100); } catch (RPCException | InterruptedException ignored) { diff --git a/src/com/pesterenan/controllers/LiftoffController.java b/src/com/pesterenan/controllers/LiftoffController.java index 77a437f..4f5259e 100644 --- a/src/com/pesterenan/controllers/LiftoffController.java +++ b/src/com/pesterenan/controllers/LiftoffController.java @@ -126,7 +126,7 @@ private void finalizeCurve() throws RPCException, StreamException, InterruptedEx private void circularizeOrbitOnApoapsis() { setCurrentStatus(Bundle.getString("status_planning_orbit")); Map commands = new HashMap<>(); - commands.put(Modulos.MODULO.get(), Modulos.MODULO_EXEC_MANOBRAS.get()); + commands.put(Modulos.MODULO.get(), Modulos.MODULE_MANEUVER.get()); commands.put(Modulos.FUNCAO.get(), Modulos.APOASTRO.get()); commands.put(Modulos.AJUSTE_FINO.get(), String.valueOf(false)); MechPeste.newInstance().startModule(commands); diff --git a/src/com/pesterenan/model/ActiveVessel.java b/src/com/pesterenan/model/ActiveVessel.java index 2fd88d1..f01f206 100644 --- a/src/com/pesterenan/model/ActiveVessel.java +++ b/src/com/pesterenan/model/ActiveVessel.java @@ -182,7 +182,7 @@ public void startModule(Map commands) { controller = new LandingController(commands); runningModule = true; } - if (currentFunction.equals(Modulos.MODULO_EXEC_MANOBRAS.get())) { + if (currentFunction.equals(Modulos.MODULE_MANEUVER.get())) { controller = new ManeuverController(commands); runningModule = true; } diff --git a/src/com/pesterenan/resources/MechPesteBundle.properties b/src/com/pesterenan/resources/MechPesteBundle.properties index e3d7143..6035da3 100644 --- a/src/com/pesterenan/resources/MechPesteBundle.properties +++ b/src/com/pesterenan/resources/MechPesteBundle.properties @@ -1,7 +1,7 @@ btn_func_landing = Auto Landing btn_func_liftoff = Orbital Liftoff btn_func_create_maneuver = Create Maneuvers -btn_func_exec_maneuver = Exec. Maneuvers +btn_func_maneuver = Maneuvers btn_func_rover = Drive Rover btn_stat_connect = Connect installer_dialog_btn_browse = Browse... diff --git a/src/com/pesterenan/resources/MechPesteBundle_pt.properties b/src/com/pesterenan/resources/MechPesteBundle_pt.properties index 2aaf5da..fbace8b 100644 --- a/src/com/pesterenan/resources/MechPesteBundle_pt.properties +++ b/src/com/pesterenan/resources/MechPesteBundle_pt.properties @@ -1,7 +1,7 @@ btn_func_landing = Pouso Autom\u00E1tico btn_func_liftoff = Decolagem Orbital btn_func_create_maneuver = Criar Manobras -btn_func_exec_maneuver = Exec. Manobras +btn_func_maneuver = Manobras btn_func_rover = Pilotar Rover btn_stat_connect = Conectar installer_dialog_btn_browse = Escolher... diff --git a/src/com/pesterenan/utils/Modulos.java b/src/com/pesterenan/utils/Modulos.java index 85222ba..17b7fc5 100644 --- a/src/com/pesterenan/utils/Modulos.java +++ b/src/com/pesterenan/utils/Modulos.java @@ -18,7 +18,7 @@ public enum Modulos { MAX_TWR("Max_TWR"), MODULO_CRIAR_MANOBRAS("CRIAR_MANOBRAS"), MODULO_DECOLAGEM("LIFTOFF"), - MODULO_EXEC_MANOBRAS("MANEUVER"), + MODULE_MANEUVER("MANEUVER"), MODULO_POUSO_SOBREVOAR("HOVER"), MODULO_POUSO("LANDING"), MODULO_ROVER("ROVER"), diff --git a/src/com/pesterenan/views/CreateManeuverJPanel.java b/src/com/pesterenan/views/CreateManeuverJPanel.java index 905a98f..23d94ea 100644 --- a/src/com/pesterenan/views/CreateManeuverJPanel.java +++ b/src/com/pesterenan/views/CreateManeuverJPanel.java @@ -2,6 +2,7 @@ import com.pesterenan.MechPeste; import com.pesterenan.resources.Bundle; +import com.pesterenan.utils.ControlePID; import krpc.client.RPCException; import krpc.client.services.SpaceCenter.Node; @@ -27,6 +28,7 @@ public class CreateManeuverJPanel extends JPanel implements ActionListener, UIMethods { + private static JLabel lblManeuverInfo; private static JButton btnCreateManeuver, btnDeleteManeuver, btnBack, btnAp, btnPe, btnAN, btnDN; private static JButton btnIncrease, btnDecrease, btnNextOrbit, btnPrevOrbit, btnAlignPlanes, btnRendezvous; private static JSlider sldScale; @@ -35,6 +37,7 @@ public class CreateManeuverJPanel extends JPanel implements ActionListener, UIMe private static JRadioButton rbPrograde, rbNormal, rbRadial, rbTime; private static ButtonGroup bgManeuverType; private static Map> sliderValues = new HashMap<>(); + private final ControlePID ctrlManeuver = new ControlePID(); public CreateManeuverJPanel() { initComponents(); @@ -44,6 +47,9 @@ public CreateManeuverJPanel() { @Override public void initComponents() { + // Labels: + lblManeuverInfo = new JLabel(""); + // Buttons: btnCreateManeuver = new JButton("Criar"); btnDeleteManeuver = new JButton("Apagar"); @@ -69,20 +75,18 @@ public void initComponents() { listCurrentManeuvers = new JList<>(); sldScale = new JSlider(JSlider.VERTICAL, 0, 5, 2); bgManeuverType = new ButtonGroup(); - sliderValues.put(0, new Pair<>("0.01m/s", 0.01f)); - sliderValues.put(1, new Pair<>("0.10m/s", 0.1f)); - sliderValues.put(2, new Pair<>("1.0 m/s", 1f)); - sliderValues.put(3, new Pair<>("10 m/s", 10f)); - sliderValues.put(4, new Pair<>("100 m/s", 100f)); - sliderValues.put(5, new Pair<>("1k m/s", 1000f)); + sliderValues.put(0, new Pair<>("0.01", 0.01f)); + sliderValues.put(1, new Pair<>("0.10", 0.1f)); + sliderValues.put(2, new Pair<>("1", 1f)); + sliderValues.put(3, new Pair<>("10", 10f)); + sliderValues.put(4, new Pair<>("100", 100f)); + sliderValues.put(5, new Pair<>("1000", 1000f)); + + ctrlManeuver.adjustOutput(-100, 100); } @Override public void setupComponents() { - // Main Panel setup: - setBorder(new TitledBorder(null, Bundle.getString("pnl_mnv_border"), TitledBorder.LEADING, TitledBorder.TOP, - null, null)); - // Setting-up components: btnCreateManeuver.addActionListener(this); btnCreateManeuver.setMaximumSize(BTN_DIMENSION); @@ -142,15 +146,29 @@ public void setupComponents() { MechPeste.newInstance(); Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); Vessel targetVessel = MechPeste.getSpaceCenter().getTargetVessel(); - List currentManeuvers = vessel.getControl().getNodes(); - if (currentManeuvers.size() < 1) { + boolean hasManeuverNodes = vessel.getControl().getNodes().size() > 0; + if (!hasManeuverNodes) { createManeuver(); - positionManeuverAt("ascending"); } + List currentManeuvers = vessel.getControl().getNodes(); Node currentManeuver = currentManeuvers.get(0); - double currentInclination = currentManeuver.getOrbit().relativeInclination(targetVessel.getOrbit()); - System.out.println(Math.toDegrees(currentInclination)); + double[] incNodesUt = { + vessel.getOrbit().uTAtTrueAnomaly(vessel.getOrbit().trueAnomalyAtAN(targetVessel.getOrbit())), + vessel.getOrbit().uTAtTrueAnomaly(vessel.getOrbit().trueAnomalyAtDN(targetVessel.getOrbit())) + }; + boolean closestIsAN = incNodesUt[0] < incNodesUt[1]; + positionManeuverAt(closestIsAN ? "ascending" : "descending"); + double currentInclination = Math + .toDegrees(currentManeuver.getOrbit().relativeInclination(targetVessel.getOrbit())); + while (currentInclination > 0.05) { + currentInclination = Math + .toDegrees(currentManeuver.getOrbit().relativeInclination(targetVessel.getOrbit())); + double ctrlOutput = ctrlManeuver.calcPID(currentInclination * 100, 0); + currentManeuver.setNormal(currentManeuver.getNormal() + (closestIsAN ? ctrlOutput : -ctrlOutput)); + Thread.sleep(25); + } } catch (Exception err) { + System.err.println(err); } }); btnRendezvous.addActionListener(e -> { @@ -158,22 +176,53 @@ public void setupComponents() { MechPeste.newInstance(); Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); Vessel targetVessel = MechPeste.getSpaceCenter().getTargetVessel(); - Node currentManeuver = vessel.getControl().getNodes().get(selectedManeuverIndex); - double closestApproach = currentManeuver.getOrbit().distanceAtClosestApproach(targetVessel.getOrbit()); + boolean hasManeuverNodes = vessel.getControl().getNodes().size() > 0; + List currentManeuvers = vessel.getControl().getNodes(); + Node lastManeuverNode; + double lastManeuverNodeUT = 60; + if (hasManeuverNodes) { + currentManeuvers = vessel.getControl().getNodes(); + lastManeuverNode = currentManeuvers.get(currentManeuvers.size() - 1); + lastManeuverNodeUT += lastManeuverNode.getUT(); + createManeuver(lastManeuverNodeUT); + } else { + createManeuver(); + } + currentManeuvers = vessel.getControl().getNodes(); + lastManeuverNode = currentManeuvers.get(currentManeuvers.size() - 1); + double targetAP = targetVessel.getOrbit().getApoapsisAltitude(); + double targetPE = targetVessel.getOrbit().getPeriapsisAltitude(); + double maneuverAP = lastManeuverNode.getOrbit().getApoapsisAltitude(); + double maneuverPE = lastManeuverNode.getOrbit().getPeriapsisAltitude(); + ctrlManeuver.adjustOutput(-10, 10); + if (targetAP < maneuverPE) { + while (Math.floor(targetAP) != Math.floor(maneuverPE)) { + lastManeuverNode.setPrograde( + lastManeuverNode.getPrograde() + ctrlManeuver.calcPID(maneuverPE - targetAP, 0)); + maneuverPE = lastManeuverNode.getOrbit().getPeriapsisAltitude(); + System.out.println("maneuverPE: " + maneuverPE); + Thread.sleep(100); + } + } + double closestApproach = lastManeuverNode.getOrbit().distanceAtClosestApproach(targetVessel.getOrbit()); System.out.println(closestApproach); } catch (Exception err) { } }); rbPrograde.setActionCommand("prograde"); + rbPrograde.addChangeListener(e -> handleChangeButtonText(sldScale.getValue())); rbNormal.setActionCommand("normal"); + rbNormal.addChangeListener(e -> handleChangeButtonText(sldScale.getValue())); rbRadial.setActionCommand("radial"); + rbRadial.addChangeListener(e -> handleChangeButtonText(sldScale.getValue())); rbTime.setActionCommand("time"); - rbPrograde.setSelected(true); + rbTime.addChangeListener(e -> handleChangeButtonText(sldScale.getValue())); bgManeuverType.add(rbPrograde); bgManeuverType.add(rbNormal); bgManeuverType.add(rbRadial); bgManeuverType.add(rbTime); + bgManeuverType.setSelected(rbPrograde.getModel(), true); listCurrentManeuvers.setSelectionMode(ListSelectionModel.SINGLE_SELECTION); listCurrentManeuvers.addListSelectionListener(e -> selectedManeuverIndex = e.getFirstIndex()); @@ -249,15 +298,15 @@ public void layoutComponents() { JPanel pnlManeuverController = new JPanel(); pnlManeuverController.setLayout(new BoxLayout(pnlManeuverController, BoxLayout.X_AXIS)); pnlManeuverController.setBorder(new TitledBorder("Controlador de Manobra:")); - pnlManeuverController.setMaximumSize(new Dimension(380, 300)); + pnlManeuverController.setMaximumSize(new Dimension(400, 300)); pnlManeuverController.add(pnlRadioButtons); pnlManeuverController.add(pnlSlider); pnlManeuverController.add(pnlManeuverButtons); - + JPanel pnlAutoPosition = new JPanel(); pnlAutoPosition.setLayout(new BoxLayout(pnlAutoPosition, BoxLayout.Y_AXIS)); pnlAutoPosition.setBorder(new TitledBorder("Auto posição:")); - pnlAutoPosition.setMaximumSize(new Dimension(400, 300)); + pnlAutoPosition.setMaximumSize(new Dimension(300, 300)); pnlAutoPosition.add(btnAlignPlanes); pnlAutoPosition.add(btnRendezvous); @@ -296,6 +345,7 @@ public void layoutComponents() { JPanel pnlBackButton = new JPanel(); pnlBackButton.setLayout(new BoxLayout(pnlBackButton, BoxLayout.X_AXIS)); + pnlBackButton.add(lblManeuverInfo); pnlBackButton.add(Box.createHorizontalGlue()); pnlBackButton.add(btnBack); @@ -307,38 +357,50 @@ public void layoutComponents() { add(pnlBackButton, BorderLayout.SOUTH); } - public static void updateManeuverList(ListModel list) { - listCurrentManeuvers.setModel(list); - listCurrentManeuvers.setSelectedIndex(selectedManeuverIndex); - btnDeleteManeuver.setEnabled(list.getSize() > 0); + public static void updatePanel(ListModel list) { try { - btnAp.setEnabled(list.getSize() > 0); - btnPe.setEnabled(list.getSize() > 0); - btnAN.setEnabled(list.getSize() > 0 && MechPeste.getSpaceCenter().getTargetVessel() != null); - btnDN.setEnabled(list.getSize() > 0 && MechPeste.getSpaceCenter().getTargetVessel() != null); + boolean hasManeuverNodes = list.getSize() > 0; + boolean hasTargetVessel = MechPeste.getSpaceCenter().getTargetVessel() != null; + listCurrentManeuvers.setModel(list); + listCurrentManeuvers.setSelectedIndex(selectedManeuverIndex); + btnDeleteManeuver.setEnabled(hasManeuverNodes); + btnAp.setEnabled(hasManeuverNodes); + btnPe.setEnabled(hasManeuverNodes); + btnAN.setEnabled(hasManeuverNodes && hasTargetVessel); + btnDN.setEnabled(hasManeuverNodes && hasTargetVessel); + btnAlignPlanes.setEnabled(hasTargetVessel); + btnIncrease.setEnabled(hasManeuverNodes); + btnDecrease.setEnabled(hasManeuverNodes); + btnNextOrbit.setEnabled(hasManeuverNodes); + btnPrevOrbit.setEnabled(hasManeuverNodes); + lblManeuverInfo.setText(listCurrentManeuvers.getSelectedValue()); } catch (RPCException ignored) { } } private static void handleChangeButtonText(int value) { - btnIncrease.setText("+ " + sliderValues.get(value).getValue0()); - btnDecrease.setText("- " + sliderValues.get(value).getValue0()); + String suffix = bgManeuverType.getSelection() == rbTime.getModel() ? " s" : "m/s"; + btnIncrease.setText("+ " + sliderValues.get(value).getValue0() + suffix); + btnDecrease.setText("- " + sliderValues.get(value).getValue0() + suffix); } private void createManeuver() { + try { + createManeuver(MechPeste.getSpaceCenter().getUT() + 60); + } catch (RPCException e) { + } + } + + private void createManeuver(double atFutureTime) { try { MechPeste.newInstance(); Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); if (vessel.getSituation() != VesselSituation.ORBITING) { - System.out.println("Não é possível criar a manobra fora de órbita."); + StatusJPanel.setStatusMessage("Não é possível criar a manobra fora de órbita."); return; } - double oneMinuteAhead = MechPeste.getSpaceCenter().getUT() + 60; - Node maneuverNode = vessel.getControl().addNode(oneMinuteAhead, 0, 0, 0); - // Print the maneuver node information - System.out.println("Maneuver Node created:"); - System.out.println(" Time to node: " + maneuverNode.getTimeTo() + " s"); + vessel.getControl().addNode(atFutureTime, 0, 0, 0); } catch (Exception e) { } } diff --git a/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java b/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java index 3aa27b5..3f0d9ac 100644 --- a/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java +++ b/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java @@ -21,7 +21,7 @@ public class FunctionsAndTelemetryJPanel extends JPanel implements UIMethods { private static final long serialVersionUID = 0L; private final Dimension btnFuncDimension = new Dimension(140, 25); - private JButton btnLiftoff, btnLanding, btnExecManeuver, btnRover, btnCancel, btnCreateManeuver; + private JButton btnLiftoff, btnLanding, btnManeuvers, btnRover, btnCancel; private static JLabel lblAltitude, lblSurfaceAlt, lblApoapsis, lblPeriapsis, lblVertSpeed, lblHorzSpeed; private static JLabel lblAltitudeValue, lblSurfaceAltValue, lblApoapsisValue; private static JLabel lblPeriapsisValue, lblVertSpeedValue, lblHorzSpeedValue; @@ -51,8 +51,7 @@ public void initComponents() { // Buttons: btnLiftoff = new JButton(Bundle.getString("btn_func_liftoff")); btnLanding = new JButton(Bundle.getString("btn_func_landing")); - btnCreateManeuver = new JButton(Bundle.getString("btn_func_create_maneuver")); - btnExecManeuver = new JButton(Bundle.getString("btn_func_exec_maneuver")); + btnManeuvers = new JButton(Bundle.getString("btn_func_maneuver")); btnRover = new JButton(Bundle.getString("btn_func_rover")); btnCancel = new JButton(Bundle.getString("pnl_tel_btn_cancel")); } @@ -76,14 +75,10 @@ public void setupComponents() { btnLiftoff.setActionCommand(Modulos.MODULO_DECOLAGEM.get()); btnLiftoff.setMaximumSize(btnFuncDimension); btnLiftoff.setPreferredSize(btnFuncDimension); - btnCreateManeuver.addActionListener(this::changeFunctionPanel); - btnCreateManeuver.setActionCommand(Modulos.MODULO_CRIAR_MANOBRAS.get()); - btnCreateManeuver.setMaximumSize(btnFuncDimension); - btnCreateManeuver.setPreferredSize(btnFuncDimension); - btnExecManeuver.addActionListener(this::changeFunctionPanel); - btnExecManeuver.setActionCommand(Modulos.MODULO_EXEC_MANOBRAS.get()); - btnExecManeuver.setMaximumSize(btnFuncDimension); - btnExecManeuver.setPreferredSize(btnFuncDimension); + btnManeuvers.addActionListener(this::changeFunctionPanel); + btnManeuvers.setActionCommand(Modulos.MODULE_MANEUVER.get()); + btnManeuvers.setMaximumSize(btnFuncDimension); + btnManeuvers.setPreferredSize(btnFuncDimension); btnRover.addActionListener(this::changeFunctionPanel); btnRover.setActionCommand(Modulos.MODULO_ROVER.get()); btnRover.setMaximumSize(btnFuncDimension); @@ -99,9 +94,7 @@ public void layoutComponents() { pnlFunctionControls.add(btnLiftoff); pnlFunctionControls.add(btnLanding); pnlFunctionControls.add(btnRover); - pnlFunctionControls.add(MainGui.createMarginComponent(0, 20)); - pnlFunctionControls.add(btnCreateManeuver); - pnlFunctionControls.add(btnExecManeuver); + pnlFunctionControls.add(btnManeuvers); pnlFunctionControls.add(Box.createVerticalGlue()); JPanel pnlLeftPanel = new JPanel(); diff --git a/src/com/pesterenan/views/MainGui.java b/src/com/pesterenan/views/MainGui.java index aedb2ee..2eab916 100644 --- a/src/com/pesterenan/views/MainGui.java +++ b/src/com/pesterenan/views/MainGui.java @@ -11,9 +11,9 @@ import java.awt.event.ActionListener; public class MainGui extends JFrame implements ActionListener, UIMethods { - + private static final long serialVersionUID = 1L; - + private final Dimension APP_DIMENSION = new Dimension(480, 300); public static final Dimension PNL_DIMENSION = new Dimension(464, 216); public static final Dimension BTN_DIMENSION = new Dimension(110, 25); @@ -26,12 +26,12 @@ public class MainGui extends JFrame implements ActionListener, UIMethods { private JMenuBar menuBar; private JMenu mnFile, mnOptions, mnHelp; private JMenuItem mntmInstallKrpc, mntmExit, mntmChangeVessels, mntmAbout; - + private final static CardLayout cardLayout = new CardLayout(0, 0); private LiftoffJPanel pnlLiftoff; private LandingJPanel pnlLanding; - private CreateManeuverJPanel pnlCreateManeuver; - private ManeuverJPanel pnlManeuver; + private CreateManeuverJPanel pnlCreateManeuvers; + private RunManeuverJPanel pnlRunManeuvers; private RoverJPanel pnlRover; private MainGui() { @@ -53,7 +53,7 @@ public void initComponents() { UIManager.setLookAndFeel(UIManager.getSystemLookAndFeelClassName()); } catch (Exception ignored) { } - + // Menu bar menuBar = new JMenuBar(); @@ -61,7 +61,7 @@ public void initComponents() { mnFile = new JMenu(Bundle.getString("main_mn_file")); mnOptions = new JMenu(Bundle.getString("main_mn_options")); mnHelp = new JMenu(Bundle.getString("main_mn_help")); - + // Menu Items mntmInstallKrpc = new JMenuItem(Bundle.getString("main_mntm_install_krpc")); mntmChangeVessels = new JMenuItem(Bundle.getString("main_mntm_change_vessels")); @@ -72,8 +72,8 @@ public void initComponents() { pnlFunctionsAndTelemetry = new FunctionsAndTelemetryJPanel(); pnlLiftoff = new LiftoffJPanel(); pnlLanding = new LandingJPanel(); - pnlCreateManeuver = new CreateManeuverJPanel(); - pnlManeuver = new ManeuverJPanel(); + pnlCreateManeuvers = new CreateManeuverJPanel(); + pnlRunManeuvers = new RunManeuverJPanel(); pnlRover = new RoverJPanel(); pnlStatus = new StatusJPanel(); } @@ -88,7 +88,7 @@ public void setupComponents() { setLocation(100, 100); setContentPane(ctpMainGui); setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); - + // Setting-up components: mntmAbout.addActionListener(this); mntmChangeVessels.addActionListener(this); @@ -98,7 +98,7 @@ public void setupComponents() { cardJPanels.setPreferredSize(PNL_DIMENSION); cardJPanels.setSize(PNL_DIMENSION); } - + @Override public void layoutComponents() { // Main Panel layout: @@ -120,12 +120,15 @@ public void layoutComponents() { menuBar.add(mnOptions); menuBar.add(mnHelp); + JTabbedPane pnlManeuverJTabbedPane = new JTabbedPane(); + pnlManeuverJTabbedPane.addTab("Criar Manobras", pnlCreateManeuvers); + pnlManeuverJTabbedPane.addTab("Executar Manobras", pnlRunManeuvers); + cardJPanels.setLayout(cardLayout); cardJPanels.add(pnlFunctionsAndTelemetry, Modulos.MODULO_TELEMETRIA.get()); cardJPanels.add(pnlLiftoff, Modulos.MODULO_DECOLAGEM.get()); cardJPanels.add(pnlLanding, Modulos.MODULO_POUSO.get()); - cardJPanels.add(pnlCreateManeuver, Modulos.MODULO_CRIAR_MANOBRAS.get()); - cardJPanels.add(pnlManeuver, Modulos.MODULO_EXEC_MANOBRAS.get()); + cardJPanels.add(pnlManeuverJTabbedPane, Modulos.MODULE_MANEUVER.get()); cardJPanels.add(pnlRover, Modulos.MODULO_ROVER.get()); } @@ -173,7 +176,7 @@ public static JPanel getCardJPanels() { public static void changeToPage(ActionEvent e) { cardLayout.show(cardJPanels, e.getActionCommand()); } - + public static void backToTelemetry(ActionEvent e) { cardLayout.show(cardJPanels, Modulos.MODULO_TELEMETRIA.get()); } diff --git a/src/com/pesterenan/views/ManeuverJPanel.java b/src/com/pesterenan/views/RunManeuverJPanel.java similarity index 94% rename from src/com/pesterenan/views/ManeuverJPanel.java rename to src/com/pesterenan/views/RunManeuverJPanel.java index c0b80e9..bfc8c48 100644 --- a/src/com/pesterenan/views/ManeuverJPanel.java +++ b/src/com/pesterenan/views/RunManeuverJPanel.java @@ -18,14 +18,14 @@ import static com.pesterenan.views.MainGui.MARGIN_BORDER_10_PX_LR; import static com.pesterenan.views.MainGui.PNL_DIMENSION; -public class ManeuverJPanel extends JPanel implements ActionListener, UIMethods { +public class RunManeuverJPanel extends JPanel implements ActionListener, UIMethods { private static final long serialVersionUID = 1L; private JLabel lblExecute, lblAdjustInc; private JButton btnLowerOrbit, btnApoapsis, btnPeriapsis, btnExecute, btnAdjustInc, btnBack; private JCheckBox chkFineAdjusment; - public ManeuverJPanel() { + public RunManeuverJPanel() { initComponents(); setupComponents(); layoutComponents(); @@ -49,10 +49,6 @@ public void initComponents() { } public void setupComponents() { - // Main Panel setup: - setBorder(new TitledBorder(null, Bundle.getString("pnl_mnv_border"), TitledBorder.LEADING, TitledBorder.TOP, - null, null)); - // Setting-up components: btnAdjustInc.addActionListener(this); btnAdjustInc.setActionCommand(Modulos.AJUSTAR.get()); @@ -171,7 +167,7 @@ public void actionPerformed(ActionEvent e) { protected void handleManeuverFunction(String maneuverFunction) { Map commands = new HashMap<>(); - commands.put(Modulos.MODULO.get(), Modulos.MODULO_EXEC_MANOBRAS.get()); + commands.put(Modulos.MODULO.get(), Modulos.MODULE_MANEUVER.get()); commands.put(Modulos.FUNCAO.get(), maneuverFunction); commands.put(Modulos.AJUSTE_FINO.get(), String.valueOf(chkFineAdjusment.isSelected())); MechPeste.newInstance().startModule(commands); From 7ac676845d0ac5f9eab5cd3870497e922786368e Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Wed, 20 Sep 2023 19:29:21 -0300 Subject: [PATCH 09/19] [MP-2]: Saving current changes to MechPeste --- src/com/pesterenan/MechPeste.java | 24 +- .../controllers/ManeuverController.java | 231 +++++++++++++----- src/com/pesterenan/utils/Attributes.java | 33 +++ src/com/pesterenan/utils/Modulos.java | 1 + .../views/CreateManeuverJPanel.java | 123 ++-------- .../pesterenan/views/RunManeuverJPanel.java | 111 +++++++-- 6 files changed, 326 insertions(+), 197 deletions(-) create mode 100644 src/com/pesterenan/utils/Attributes.java diff --git a/src/com/pesterenan/MechPeste.java b/src/com/pesterenan/MechPeste.java index ee247fa..71f917c 100644 --- a/src/com/pesterenan/MechPeste.java +++ b/src/com/pesterenan/MechPeste.java @@ -1,11 +1,23 @@ package com.pesterenan; +import static com.pesterenan.views.StatusJPanel.isBtnConnectVisible; +import static com.pesterenan.views.StatusJPanel.setStatusMessage; + +import java.awt.event.ActionEvent; +import java.io.IOException; +import java.util.List; +import java.util.Map; +import java.util.stream.Collectors; + +import javax.swing.*; + import com.pesterenan.model.ActiveVessel; import com.pesterenan.resources.Bundle; import com.pesterenan.utils.Vector; import com.pesterenan.views.CreateManeuverJPanel; import com.pesterenan.views.FunctionsAndTelemetryJPanel; import com.pesterenan.views.MainGui; + import krpc.client.Connection; import krpc.client.RPCException; import krpc.client.services.KRPC; @@ -13,16 +25,6 @@ import krpc.client.services.SpaceCenter.Node; import krpc.client.services.SpaceCenter.Vessel; -import javax.swing.*; -import java.awt.event.ActionEvent; -import java.io.IOException; -import java.util.List; -import java.util.Map; -import java.util.stream.Collectors; - -import static com.pesterenan.views.StatusJPanel.isBtnConnectVisible; -import static com.pesterenan.views.StatusJPanel.setStatusMessage; - public class MechPeste { private static KRPC krpc; private static MechPeste mechPeste; @@ -206,4 +208,4 @@ public void checkConnection() { public static void cancelControl(ActionEvent e) { currentVessel.cancelControl(); } -} \ No newline at end of file +} diff --git a/src/com/pesterenan/controllers/ManeuverController.java b/src/com/pesterenan/controllers/ManeuverController.java index 745e24f..98449a5 100644 --- a/src/com/pesterenan/controllers/ManeuverController.java +++ b/src/com/pesterenan/controllers/ManeuverController.java @@ -1,9 +1,13 @@ package com.pesterenan.controllers; import com.pesterenan.resources.Bundle; +import com.pesterenan.utils.Attributes; import com.pesterenan.utils.ControlePID; import com.pesterenan.utils.Modulos; import com.pesterenan.utils.Navigation; +import com.pesterenan.utils.Vector; +import com.pesterenan.views.RunManeuverJPanel; + import krpc.client.RPCException; import krpc.client.Stream; import krpc.client.StreamException; @@ -11,6 +15,8 @@ import krpc.client.services.SpaceCenter.Node; import krpc.client.services.SpaceCenter.Orbit; import krpc.client.services.SpaceCenter.RCS; +import krpc.client.services.SpaceCenter.ReferenceFrame; +import krpc.client.services.SpaceCenter.Vessel; import krpc.client.services.SpaceCenter.VesselSituation; import org.javatuples.Triplet; @@ -39,30 +45,60 @@ public ManeuverController(Map commands) { private void initializeParameters() { ctrlRCS.adjustOutput(0.5, 1.0); fineAdjustment = canFineAdjust(commands.get(Modulos.AJUSTE_FINO.get())); - lowOrbitAltitude = calculateSafeLowOrbitAltitude(); + try { + lowOrbitAltitude = new Attributes().getLowOrbitAltitude(currentBody.getName()); + System.out.println("lowOrbitAltitude: " + lowOrbitAltitude); + } catch (RPCException e) { + } } @Override public void run() { calculateManeuver(); - executeNextManeuver(); + if (!(commands.get(Modulos.FUNCAO.get()).equals(Modulos.RENDEZVOUS.get()) + || commands.get(Modulos.FUNCAO.get()).equals(Modulos.ORBITA_BAIXA.get()) + || commands.get(Modulos.FUNCAO.get()).equals(Modulos.AJUSTAR.get()))) { + executeNextManeuver(); + } } - private double calculateSafeLowOrbitAltitude() { - final double safeAltitude = 20000; - double bodyRadius = 0, atmosphereDepth = 0; + private Node biEllipticTransferToOrbit(double targetAltitude, double timeToStart) { + double[] totalDv = { 0, 0, 0 }; try { - bodyRadius = currentBody.getEquatorialRadius(); - atmosphereDepth = currentBody.getAtmosphereDepth(); - } catch (RPCException ignored) { + Orbit currentOrbit = getNaveAtual().getOrbit(); + double startingRadius = currentOrbit.getApoapsis(); + double gravParameter = currentBody.getGravitationalParameter(); + + // Delta-v required to leave the current orbit + double deltaV1 = Math.sqrt(2 * gravParameter / startingRadius) - Math.sqrt(gravParameter / startingRadius); + + // Calculate the intermediate radius for the intermediate orbit + double intermediateRadius = currentBody.getEquatorialRadius() + targetAltitude; + + // Delta-v required to enter the intermediate orbit + double deltaV2 = Math.sqrt(gravParameter / intermediateRadius) + - Math.sqrt(2 * gravParameter / intermediateRadius); + + // Calculate the final radius for the target orbit + double targetRadius = currentBody.getEquatorialRadius() + targetAltitude; + + // Delta-v required to leave the intermediate orbit and enter the target orbit + double deltaV3 = Math.sqrt(2 * gravParameter / intermediateRadius) + - Math.sqrt(gravParameter / intermediateRadius); + double deltaV4 = Math.sqrt(gravParameter / targetRadius) - Math.sqrt(2 * gravParameter / targetRadius); + + // Total delta-v for the bi-elliptic transfer + totalDv[0] = deltaV1 + deltaV2 + deltaV3 + deltaV4; + } catch (RPCException e) { + // Handle the exception } - return bodyRadius + (atmosphereDepth > 0 ? atmosphereDepth + safeAltitude : safeAltitude); + return createManeuver(timeToStart, totalDv); } public void calculateManeuver() { try { tuneAutoPilot(); - System.out.println(commands); + System.out.println(commands + " calculate maneuvers"); if (commands.get(Modulos.FUNCAO.get()).equals(Modulos.EXECUTAR.get())) { return; } @@ -71,12 +107,15 @@ public void calculateManeuver() { throw new InterruptedException(); } if (commands.get(Modulos.FUNCAO.get()).equals(Modulos.AJUSTAR.get())) { - this.alignPlanes(); + this.alignPlanesWithTargetVessel(); + return; + } + if (commands.get(Modulos.FUNCAO.get()).equals(Modulos.RENDEZVOUS.get())) { + this.rendezvousWithTargetVessel(); return; } if (commands.get(Modulos.FUNCAO.get()).equals(Modulos.ORBITA_BAIXA.get())) { - hohmannTransferToOrbit(lowOrbitAltitude, getNaveAtual().getOrbit().getTimeToPeriapsis()); - hohmannTransferToOrbit(lowOrbitAltitude, getNaveAtual().getOrbit().getTimeToPeriapsis()); + biEllipticTransferToOrbit(lowOrbitAltitude, getNaveAtual().getOrbit().getTimeToPeriapsis()); return; } double gravParameter = currentBody.getGravitationalParameter(); @@ -105,7 +144,7 @@ public void matchOrbitApoapsis() { try { Orbit targetOrbit = getTargetOrbit(); System.out.println(targetOrbit.getApoapsis() + "-- APO"); - Node maneuver = hohmannTransferToOrbit(targetOrbit.getApoapsis(), + Node maneuver = biEllipticTransferToOrbit(targetOrbit.getApoapsis(), getNaveAtual().getOrbit().getTimeToPeriapsis()); while (true) { if (Thread.interrupted()) { @@ -128,27 +167,6 @@ public void matchOrbitApoapsis() { } } - private Node hohmannTransferToOrbit(double targetAltitude, double timeToStart) { - double[] totalDv = { 0, 0, 0 }; - try { - double startingRadius = Math.max(getNaveAtual().getOrbit().getApoapsis(), - getNaveAtual().getOrbit().getPeriapsis()); - System.out.println(startingRadius + " --- " + targetAltitude); - double gravParameter = currentBody.getGravitationalParameter(); - // Delta-v required to leave the current orbit - double deltaV1 = Math.sqrt(2 * gravParameter / startingRadius) - Math.sqrt(gravParameter / startingRadius); - // Delta-v required to enter the target orbit - double deltaV2 = Math.sqrt(gravParameter / targetAltitude) - Math.sqrt(2 * gravParameter / targetAltitude); - System.out.println(deltaV1); - System.out.println(deltaV2); - - // Dv taken between the two points - totalDv[0] = deltaV2 + deltaV1; - } catch (RPCException e) { - } - return createManeuver(timeToStart, totalDv); - } - private Node createManeuverAtClosestIncNode(Orbit targetOrbit) { double uTatClosestNode = 1; double[] dv = { 0, 0, 0 }; @@ -167,36 +185,121 @@ private double[] getTimeToIncNodes(Orbit targetOrbit) throws RPCException { return new double[] { vesselOrbit.uTAtTrueAnomaly(ascendingNode), vesselOrbit.uTAtTrueAnomaly(descendingNode) }; } - public void alignPlanes() { + private void alignPlanesWithTargetVessel() { try { - Orbit targetOrbit = getTargetOrbit(); - Node maneuver = createManeuverAtClosestIncNode(targetOrbit); - double[] incNodesUt = getTimeToIncNodes(targetOrbit); + Vessel vessel = getNaveAtual(); + Orbit vesselOrbit = getNaveAtual().getOrbit(); + Orbit targetVesselOrbit = getSpaceCenter().getTargetVessel().getOrbit(); + boolean hasManeuverNodes = vessel.getControl().getNodes().size() > 0; + System.out.println("hasManeuverNodes: " + hasManeuverNodes); + if (!hasManeuverNodes) { + RunManeuverJPanel.createManeuver(); + } + java.util.List currentManeuvers = vessel.getControl().getNodes(); + Node currentManeuver = currentManeuvers.get(0); + double[] incNodesUt = { + vesselOrbit.uTAtTrueAnomaly(vesselOrbit.trueAnomalyAtAN(targetVesselOrbit)), + vesselOrbit.uTAtTrueAnomaly(vesselOrbit.trueAnomalyAtDN(targetVesselOrbit)) + }; boolean closestIsAN = incNodesUt[0] < incNodesUt[1]; - double timeToExecute = 0; - while (timeToExecute < 5000) { - if (Thread.interrupted()) { - throw new InterruptedException(); + RunManeuverJPanel.positionManeuverAt(closestIsAN ? "ascending" : "descending"); + double currentInclination = Math + .toDegrees(currentManeuver.getOrbit().relativeInclination(targetVesselOrbit)); + while (currentInclination > 0.05) { + currentInclination = Math + .toDegrees(currentManeuver.getOrbit().relativeInclination(targetVesselOrbit)); + double ctrlOutput = ctrlManeuver.calcPID(currentInclination * 100, 0); + currentManeuver.setNormal(currentManeuver.getNormal() + (closestIsAN ? ctrlOutput : -ctrlOutput)); + Thread.sleep(25); + } + } catch (Exception err) { + System.err.println(err); + } + } + + private void rendezvousWithTargetVessel() { + try { + Orbit targetVesselOrbit = getSpaceCenter().getTargetVessel().getOrbit(); + boolean hasManeuverNodes = getNaveAtual().getControl().getNodes().size() > 0; + java.util.List currentManeuvers = getNaveAtual().getControl().getNodes(); + Node lastManeuverNode; + double lastManeuverNodeUT = 60; + if (hasManeuverNodes) { + currentManeuvers = getNaveAtual().getControl().getNodes(); + lastManeuverNode = currentManeuvers.get(currentManeuvers.size() - 1); + lastManeuverNodeUT += lastManeuverNode.getUT(); + RunManeuverJPanel.createManeuver(lastManeuverNodeUT); + } else { + RunManeuverJPanel.createManeuver(); + } + currentManeuvers = getNaveAtual().getControl().getNodes(); + lastManeuverNode = currentManeuvers.get(currentManeuvers.size() - 1); + double targetAP = targetVesselOrbit.getApoapsis(); + double targetPE = targetVesselOrbit.getPeriapsis(); + double maneuverAP = lastManeuverNode.getOrbit().getApoapsis(); + double maneuverPE = lastManeuverNode.getOrbit().getPeriapsis(); + double maneuverUT = lastManeuverNode.getUT(); + ctrlManeuver.adjustPID(0.25, 0.0, 0.01); + ctrlManeuver.adjustOutput(-100, 100); + if (targetAP < maneuverPE) { + while (Math.floor(targetAP) != Math.floor(maneuverPE)) { + lastManeuverNode.setPrograde( + lastManeuverNode.getPrograde() + + ctrlManeuver.calcPID(maneuverPE / targetAP * 1000, 1000)); + maneuverPE = lastManeuverNode.getOrbit().getPeriapsis(); + Thread.sleep(25); } - double currentDeltaInc = compareOrbitParameter(maneuver.getOrbit(), targetOrbit, Compare.INC); - String deltaIncFormatted = String.format("%.2f", currentDeltaInc); - System.out.println(deltaIncFormatted); - if (deltaIncFormatted.equals(String.format("%.2f", 10.00))) { - break; + } + if (targetPE > maneuverAP) { + while (Math.floor(targetPE) != Math.floor(maneuverAP)) { + lastManeuverNode.setPrograde( + lastManeuverNode.getPrograde() + + ctrlManeuver.calcPID(maneuverAP / targetPE * 1000, 1000)); + maneuverAP = lastManeuverNode.getOrbit().getApoapsis(); + Thread.sleep(25); } - double dvNormal = maneuver.getNormal(); - double ctrlOutput = ctrlManeuver.calcPID(currentDeltaInc, 10.0);// * limitPIDOutput(Math.abs - // (currentDeltaInc)); - if ((closestIsAN ? currentDeltaInc : -currentDeltaInc) > 0.0) { - maneuver.setNormal(dvNormal + (ctrlOutput)); + } + + double mu = currentBody.getGravitationalParameter(); + double time = 1000; + + double hohmannTransferDistance = lastManeuverNode.getOrbit().getSemiMajorAxis(); + double timeOfFlight = Math.PI * Math.sqrt(Math.pow(hohmannTransferDistance, 3) / mu); + double angle = getNaveAtual().getOrbit().getMeanAnomalyAtEpoch(); + double omegaInterceptor = Math + .sqrt(mu / Math.pow(getNaveAtual().getOrbit().radiusAt(getSpaceCenter().getUT()), 3)); // rad/s + double omegaTarget = Math.sqrt(mu / Math.pow(targetVesselOrbit.radiusAt(getSpaceCenter().getUT()), 3)); // rad/s + // double leadAngle = omegaTarget * timeOfFlight; // rad + double leadAngle = targetVesselOrbit.getMeanAnomalyAtEpoch(); // rad + double phaseAngle = Math.PI - leadAngle; // rad + double calcAngle = (phaseAngle - angle); + calcAngle = calcAngle < 0 ? calcAngle + (Math.PI * 2) : calcAngle; + double waitTime = calcAngle / (omegaTarget - omegaInterceptor); + time = waitTime; + + lastManeuverNode.setUT(getSpaceCenter().getUT() + time); + ctrlManeuver.adjustOutput(-100, 100); + ctrlManeuver.adjustPID(0.05, 0.1, 0.01); + double closestApproach = lastManeuverNode.getOrbit().distanceAtClosestApproach(targetVesselOrbit); + System.out.println(closestApproach); + System.out.println("Ajustando tempo de Rendezvous..."); + while (Math.round(closestApproach) > 100) { + if (closestApproach < 100000) { + ctrlManeuver.adjustOutput(-10, 10); + } else if (closestApproach < 10000) { + ctrlManeuver.adjustOutput(-1, 1); } else { - maneuver.setNormal(dvNormal - (ctrlOutput)); + ctrlManeuver.adjustOutput(-100, 100); } - timeToExecute += 25; + maneuverUT = ctrlManeuver.calcPID(-closestApproach, 0); + lastManeuverNode.setUT(lastManeuverNode.getUT() + maneuverUT); + System.out.println("Closest " + (closestApproach)); + closestApproach = targetVesselOrbit.distanceAtClosestApproach(lastManeuverNode.getOrbit()); Thread.sleep(25); } - } catch (Exception e) { - setCurrentStatus("Não foi possivel ajustar a inclinação"); + // lastManeuverNode.setUT(lastManeuverNode.getUT() - + // lastManeuverNode.getOrbit().getPeriod() / 2); + } catch (Exception err) { } } @@ -325,31 +428,33 @@ public void executeBurn(Node noDeManobra, double duracaoDaQueima) { Thread.sleep(100); } // Executar a manobra: - Stream> queimaRestante = getConnection().addStream(noDeManobra, + Stream> remainingBurn = getConnection().addStream(noDeManobra, "remainingBurnVector", noDeManobra.getReferenceFrame()); setCurrentStatus(Bundle.getString("status_maneuver_executing")); while (noDeManobra != null) { + double burnDvLeft = remainingBurn.get().getValue1(); if (Thread.interrupted()) { throw new InterruptedException(); } - if (queimaRestante.get().getValue1() < (fineAdjustment ? 2 : 0.5)) { + if (burnDvLeft < (fineAdjustment ? 2 : 0.5)) { throttle(0.0f); break; } navigation.aimAtManeuver(noDeManobra); - throttle(ctrlManeuver.calcPID((noDeManobra.getDeltaV() - Math.floor(queimaRestante.get().getValue1())) / - noDeManobra.getDeltaV() * 1000, 1000)); + float limitValue = burnDvLeft > 100 ? 1000 : 100; + throttle(ctrlManeuver.calcPID((noDeManobra.getDeltaV() - Math.floor(burnDvLeft)) / + noDeManobra.getDeltaV() * limitValue, limitValue)); Thread.sleep(50); } throttle(0.0f); if (fineAdjustment) { - adjustManeuverWithRCS(queimaRestante); + adjustManeuverWithRCS(remainingBurn); } ap.setReferenceFrame(surfaceReferenceFrame); ap.disengage(); getNaveAtual().getControl().setSAS(true); getNaveAtual().getControl().setRCS(false); - queimaRestante.remove(); + remainingBurn.remove(); noDeManobra.remove(); setCurrentStatus(Bundle.getString("status_ready")); } catch (StreamException | RPCException e) { diff --git a/src/com/pesterenan/utils/Attributes.java b/src/com/pesterenan/utils/Attributes.java new file mode 100644 index 0000000..1046b85 --- /dev/null +++ b/src/com/pesterenan/utils/Attributes.java @@ -0,0 +1,33 @@ +package com.pesterenan.utils; + +import java.util.HashMap; +import java.util.Map; + +public class Attributes { + + private static Map safeLowOrbitAltitudes = new HashMap(); + + public Attributes() { + safeLowOrbitAltitudes.put("Bop", 10_000.0); + safeLowOrbitAltitudes.put("Dres", 20_000.0); + safeLowOrbitAltitudes.put("Duna", 60_000.0); + safeLowOrbitAltitudes.put("Eeloo", 20_000.0); + safeLowOrbitAltitudes.put("Eve", 100_000.0); + safeLowOrbitAltitudes.put("Gilly", 10_000.0); + safeLowOrbitAltitudes.put("Ike", 20_000.0); + safeLowOrbitAltitudes.put("Jool", 220_000.0); + safeLowOrbitAltitudes.put("Kerbin", 80_000.0); + safeLowOrbitAltitudes.put("Laythe", 80_000.0); + safeLowOrbitAltitudes.put("Minmus", 10_000.0); + safeLowOrbitAltitudes.put("Moho", 35_000.0); + safeLowOrbitAltitudes.put("Mun", 10_000.0); + safeLowOrbitAltitudes.put("Pol", 10_000.0); + safeLowOrbitAltitudes.put("Sun", 350_000.0); + safeLowOrbitAltitudes.put("Tylo", 40_000.0); + safeLowOrbitAltitudes.put("Vall", 20_000.0); + } + + public double getLowOrbitAltitude(String celestialBody) { + return safeLowOrbitAltitudes.getOrDefault(celestialBody, 10_000.0); + }; +} diff --git a/src/com/pesterenan/utils/Modulos.java b/src/com/pesterenan/utils/Modulos.java index 17b7fc5..5bdf85c 100644 --- a/src/com/pesterenan/utils/Modulos.java +++ b/src/com/pesterenan/utils/Modulos.java @@ -30,6 +30,7 @@ public enum Modulos { PERIASTRO("Periastro"), POUSAR("Pousar nave"), QUADRATICA("Quadrática"), + RENDEZVOUS("Rendezvous"), ROLAGEM("Rolagem"), SINUSOIDAL("Sinusoidal"), SOBREVOO_POS_POUSO("SOBREVOO PÓS POUSO"), diff --git a/src/com/pesterenan/views/CreateManeuverJPanel.java b/src/com/pesterenan/views/CreateManeuverJPanel.java index 23d94ea..db253de 100644 --- a/src/com/pesterenan/views/CreateManeuverJPanel.java +++ b/src/com/pesterenan/views/CreateManeuverJPanel.java @@ -20,7 +20,7 @@ import java.awt.event.ActionListener; import java.util.HashMap; import java.util.Hashtable; -import java.util.List; +import java.util.Locale; import java.util.Map; import static com.pesterenan.views.MainGui.PNL_DIMENSION; @@ -30,13 +30,13 @@ public class CreateManeuverJPanel extends JPanel implements ActionListener, UIMe private static JLabel lblManeuverInfo; private static JButton btnCreateManeuver, btnDeleteManeuver, btnBack, btnAp, btnPe, btnAN, btnDN; - private static JButton btnIncrease, btnDecrease, btnNextOrbit, btnPrevOrbit, btnAlignPlanes, btnRendezvous; + private static JButton btnIncrease, btnDecrease, btnNextOrbit, btnPrevOrbit; private static JSlider sldScale; private static JList listCurrentManeuvers; private static int selectedManeuverIndex = 0; private static JRadioButton rbPrograde, rbNormal, rbRadial, rbTime; private static ButtonGroup bgManeuverType; - private static Map> sliderValues = new HashMap<>(); + private static Map sliderValues = new HashMap<>(); private final ControlePID ctrlManeuver = new ControlePID(); public CreateManeuverJPanel() { @@ -62,8 +62,6 @@ public void initComponents() { btnDecrease = new JButton("-"); btnNextOrbit = new JButton(">"); btnPrevOrbit = new JButton("<"); - btnAlignPlanes = new JButton("Alinhar planos"); - btnRendezvous = new JButton("Rendezvous"); // Radio buttons: rbPrograde = new JRadioButton("Pro"); @@ -75,12 +73,12 @@ public void initComponents() { listCurrentManeuvers = new JList<>(); sldScale = new JSlider(JSlider.VERTICAL, 0, 5, 2); bgManeuverType = new ButtonGroup(); - sliderValues.put(0, new Pair<>("0.01", 0.01f)); - sliderValues.put(1, new Pair<>("0.10", 0.1f)); - sliderValues.put(2, new Pair<>("1", 1f)); - sliderValues.put(3, new Pair<>("10", 10f)); - sliderValues.put(4, new Pair<>("100", 100f)); - sliderValues.put(5, new Pair<>("1000", 1000f)); + sliderValues.put(0, 0.01f); + sliderValues.put(1, 0.1f); + sliderValues.put(2, 1f); + sliderValues.put(3, 10f); + sliderValues.put(4, 100f); + sliderValues.put(5, 1000f); ctrlManeuver.adjustOutput(-100, 100); } @@ -141,74 +139,6 @@ public void setupComponents() { btnPrevOrbit.setMaximumSize(new Dimension(35, 26)); btnPrevOrbit.setPreferredSize(new Dimension(35, 26)); btnPrevOrbit.setMargin(new Insets(0, 0, 0, 0)); - btnAlignPlanes.addActionListener(e -> { - try { - MechPeste.newInstance(); - Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); - Vessel targetVessel = MechPeste.getSpaceCenter().getTargetVessel(); - boolean hasManeuverNodes = vessel.getControl().getNodes().size() > 0; - if (!hasManeuverNodes) { - createManeuver(); - } - List currentManeuvers = vessel.getControl().getNodes(); - Node currentManeuver = currentManeuvers.get(0); - double[] incNodesUt = { - vessel.getOrbit().uTAtTrueAnomaly(vessel.getOrbit().trueAnomalyAtAN(targetVessel.getOrbit())), - vessel.getOrbit().uTAtTrueAnomaly(vessel.getOrbit().trueAnomalyAtDN(targetVessel.getOrbit())) - }; - boolean closestIsAN = incNodesUt[0] < incNodesUt[1]; - positionManeuverAt(closestIsAN ? "ascending" : "descending"); - double currentInclination = Math - .toDegrees(currentManeuver.getOrbit().relativeInclination(targetVessel.getOrbit())); - while (currentInclination > 0.05) { - currentInclination = Math - .toDegrees(currentManeuver.getOrbit().relativeInclination(targetVessel.getOrbit())); - double ctrlOutput = ctrlManeuver.calcPID(currentInclination * 100, 0); - currentManeuver.setNormal(currentManeuver.getNormal() + (closestIsAN ? ctrlOutput : -ctrlOutput)); - Thread.sleep(25); - } - } catch (Exception err) { - System.err.println(err); - } - }); - btnRendezvous.addActionListener(e -> { - try { - MechPeste.newInstance(); - Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); - Vessel targetVessel = MechPeste.getSpaceCenter().getTargetVessel(); - boolean hasManeuverNodes = vessel.getControl().getNodes().size() > 0; - List currentManeuvers = vessel.getControl().getNodes(); - Node lastManeuverNode; - double lastManeuverNodeUT = 60; - if (hasManeuverNodes) { - currentManeuvers = vessel.getControl().getNodes(); - lastManeuverNode = currentManeuvers.get(currentManeuvers.size() - 1); - lastManeuverNodeUT += lastManeuverNode.getUT(); - createManeuver(lastManeuverNodeUT); - } else { - createManeuver(); - } - currentManeuvers = vessel.getControl().getNodes(); - lastManeuverNode = currentManeuvers.get(currentManeuvers.size() - 1); - double targetAP = targetVessel.getOrbit().getApoapsisAltitude(); - double targetPE = targetVessel.getOrbit().getPeriapsisAltitude(); - double maneuverAP = lastManeuverNode.getOrbit().getApoapsisAltitude(); - double maneuverPE = lastManeuverNode.getOrbit().getPeriapsisAltitude(); - ctrlManeuver.adjustOutput(-10, 10); - if (targetAP < maneuverPE) { - while (Math.floor(targetAP) != Math.floor(maneuverPE)) { - lastManeuverNode.setPrograde( - lastManeuverNode.getPrograde() + ctrlManeuver.calcPID(maneuverPE - targetAP, 0)); - maneuverPE = lastManeuverNode.getOrbit().getPeriapsisAltitude(); - System.out.println("maneuverPE: " + maneuverPE); - Thread.sleep(100); - } - } - double closestApproach = lastManeuverNode.getOrbit().distanceAtClosestApproach(targetVessel.getOrbit()); - System.out.println(closestApproach); - } catch (Exception err) { - } - }); rbPrograde.setActionCommand("prograde"); rbPrograde.addChangeListener(e -> handleChangeButtonText(sldScale.getValue())); @@ -231,15 +161,7 @@ public void setupComponents() { sldScale.setPaintTicks(true); sldScale.setMajorTickSpacing(1); sldScale.setMinorTickSpacing(1); - Hashtable labelTable = new Hashtable(); - labelTable.put(0, new JLabel(sliderValues.get(0).getValue0())); - labelTable.put(1, new JLabel(sliderValues.get(1).getValue0())); - labelTable.put(2, new JLabel(sliderValues.get(2).getValue0())); - labelTable.put(3, new JLabel(sliderValues.get(3).getValue0())); - labelTable.put(4, new JLabel(sliderValues.get(4).getValue0())); - labelTable.put(5, new JLabel(sliderValues.get(5).getValue0())); - sldScale.setLabelTable(labelTable); - sldScale.setPaintLabels(true); + sldScale.setPaintLabels(false); sldScale.addChangeListener(e -> handleChangeButtonText(sldScale.getValue())); sldScale.addMouseWheelListener(e -> { int rotation = e.getWheelRotation(); @@ -279,8 +201,8 @@ public void layoutComponents() { pnlSlider.setLayout(new BoxLayout(pnlSlider, BoxLayout.Y_AXIS)); pnlSlider.setAlignmentX(Component.LEFT_ALIGNMENT); pnlSlider.setBorder(new TitledBorder("Escala:")); - pnlSlider.setPreferredSize(new Dimension(50, 100)); pnlSlider.add(sldScale); + pnlSlider.add(Box.createHorizontalStrut(40)); JPanel pnlOrbitControl = new JPanel(); pnlOrbitControl.setLayout(new BoxLayout(pnlOrbitControl, BoxLayout.X_AXIS)); @@ -298,22 +220,20 @@ public void layoutComponents() { JPanel pnlManeuverController = new JPanel(); pnlManeuverController.setLayout(new BoxLayout(pnlManeuverController, BoxLayout.X_AXIS)); pnlManeuverController.setBorder(new TitledBorder("Controlador de Manobra:")); - pnlManeuverController.setMaximumSize(new Dimension(400, 300)); + pnlManeuverController.setMaximumSize(new Dimension(180, 300)); pnlManeuverController.add(pnlRadioButtons); pnlManeuverController.add(pnlSlider); pnlManeuverController.add(pnlManeuverButtons); - JPanel pnlAutoPosition = new JPanel(); - pnlAutoPosition.setLayout(new BoxLayout(pnlAutoPosition, BoxLayout.Y_AXIS)); - pnlAutoPosition.setBorder(new TitledBorder("Auto posição:")); - pnlAutoPosition.setMaximumSize(new Dimension(300, 300)); - pnlAutoPosition.add(btnAlignPlanes); - pnlAutoPosition.add(btnRendezvous); + JPanel pnlManeuverInfo = new JPanel(); + pnlManeuverInfo.setLayout(new BoxLayout(pnlManeuverInfo, BoxLayout.Y_AXIS)); + pnlManeuverInfo.setBorder(new TitledBorder("Info. Manobra:")); + pnlManeuverInfo.setMaximumSize(new Dimension(300, 300)); JPanel pnlMCpnlAP = new JPanel(); pnlMCpnlAP.setLayout(new BoxLayout(pnlMCpnlAP, BoxLayout.X_AXIS)); pnlMCpnlAP.add(pnlManeuverController); - pnlMCpnlAP.add(pnlAutoPosition); + pnlMCpnlAP.add(pnlManeuverInfo); JPanel pnlControls = new JPanel(); pnlControls.setLayout(new BoxLayout(pnlControls, BoxLayout.Y_AXIS)); @@ -368,7 +288,6 @@ public static void updatePanel(ListModel list) { btnPe.setEnabled(hasManeuverNodes); btnAN.setEnabled(hasManeuverNodes && hasTargetVessel); btnDN.setEnabled(hasManeuverNodes && hasTargetVessel); - btnAlignPlanes.setEnabled(hasTargetVessel); btnIncrease.setEnabled(hasManeuverNodes); btnDecrease.setEnabled(hasManeuverNodes); btnNextOrbit.setEnabled(hasManeuverNodes); @@ -379,9 +298,11 @@ public static void updatePanel(ListModel list) { } private static void handleChangeButtonText(int value) { + String decimalPlaces = value > 1 ? "%.0f" : "%.2f"; + String formattedValue = String.format(Locale.ENGLISH, decimalPlaces, sliderValues.get(value)); String suffix = bgManeuverType.getSelection() == rbTime.getModel() ? " s" : "m/s"; - btnIncrease.setText("+ " + sliderValues.get(value).getValue0() + suffix); - btnDecrease.setText("- " + sliderValues.get(value).getValue0() + suffix); + btnIncrease.setText("+ " + formattedValue + suffix); + btnDecrease.setText("- " + formattedValue + suffix); } private void createManeuver() { @@ -454,7 +375,7 @@ private void changeManeuverDeltaV(String command) { Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); Node currentManeuver = vessel.getControl().getNodes().get(selectedManeuverIndex); String maneuverType = bgManeuverType.getSelection().getActionCommand(); - float currentSliderValue = sliderValues.get(sldScale.getValue()).getValue1(); + float currentSliderValue = sliderValues.get(sldScale.getValue()); currentSliderValue = command == "decrease" ? -currentSliderValue : currentSliderValue; switch (maneuverType) { diff --git a/src/com/pesterenan/views/RunManeuverJPanel.java b/src/com/pesterenan/views/RunManeuverJPanel.java index bfc8c48..ab2f7ee 100644 --- a/src/com/pesterenan/views/RunManeuverJPanel.java +++ b/src/com/pesterenan/views/RunManeuverJPanel.java @@ -2,8 +2,17 @@ import com.pesterenan.MechPeste; import com.pesterenan.resources.Bundle; +import com.pesterenan.utils.ControlePID; +import com.pesterenan.utils.Vector; import com.pesterenan.utils.Modulos; +import krpc.client.RPCException; +import krpc.client.services.SpaceCenter.Node; +import krpc.client.services.SpaceCenter.Orbit; +import krpc.client.services.SpaceCenter.ReferenceFrame; +import krpc.client.services.SpaceCenter.Vessel; +import krpc.client.services.SpaceCenter.VesselSituation; + import javax.swing.*; import javax.swing.border.CompoundBorder; import javax.swing.border.TitledBorder; @@ -21,9 +30,10 @@ public class RunManeuverJPanel extends JPanel implements ActionListener, UIMethods { private static final long serialVersionUID = 1L; - private JLabel lblExecute, lblAdjustInc; - private JButton btnLowerOrbit, btnApoapsis, btnPeriapsis, btnExecute, btnAdjustInc, btnBack; + private JLabel lblExecute; + private JButton btnLowerOrbit, btnApoapsis, btnPeriapsis, btnExecute, btnBack, btnAlignPlanes, btnRendezvous; private JCheckBox chkFineAdjusment; + private final ControlePID ctrlManeuver = new ControlePID(); public RunManeuverJPanel() { initComponents(); @@ -33,31 +43,89 @@ public RunManeuverJPanel() { public void initComponents() { // Labels: - lblAdjustInc = new JLabel(Bundle.getString("pnl_mnv_lbl_adj_inc")); lblExecute = new JLabel(Bundle.getString("pnl_mnv_lbl_exec_mnv")); // Buttons: - btnAdjustInc = new JButton(Bundle.getString("pnl_mnv_btn_adj_inc")); btnApoapsis = new JButton(Bundle.getString("pnl_mnv_btn_apoapsis")); btnBack = new JButton(Bundle.getString("pnl_mnv_btn_back")); btnExecute = new JButton(Bundle.getString("pnl_mnv_btn_exec_mnv")); btnLowerOrbit = new JButton(Bundle.getString("pnl_mnv_btn_lower_orbit")); btnPeriapsis = new JButton(Bundle.getString("pnl_mnv_btn_periapsis")); + btnAlignPlanes = new JButton("Alinhar planos"); + btnRendezvous = new JButton("Rendezvous"); // Misc: chkFineAdjusment = new JCheckBox(Bundle.getString("pnl_mnv_chk_adj_mnv_rcs")); } + public static void createManeuver() { + System.out.println("Create maneuver"); + try { + createManeuver(MechPeste.getSpaceCenter().getUT() + 60); + } catch (RPCException e) { + } + } + + public static void createManeuver(double atFutureTime) { + System.out.println("Create maneuver overloaded"); + try { + MechPeste.newInstance(); + Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); + System.out.println("vessel: " + vessel); + + if (vessel.getSituation() != VesselSituation.ORBITING) { + StatusJPanel.setStatusMessage("Não é possível criar a manobra fora de órbita."); + return; + } + vessel.getControl().addNode(atFutureTime, 0, 0, 0); + } catch (Exception e) { + } + } + + public static void positionManeuverAt(String node) { + try { + MechPeste.newInstance(); + Vessel vessel = MechPeste.getSpaceCenter().getActiveVessel(); + Orbit orbit = vessel.getOrbit(); + Node currentManeuver = vessel.getControl().getNodes().get(0); + double timeToNode = 0; + switch (node) { + case "apoapsis": + timeToNode = MechPeste.getSpaceCenter().getUT() + orbit.getTimeToApoapsis(); + break; + case "periapsis": + timeToNode = MechPeste.getSpaceCenter().getUT() + orbit.getTimeToPeriapsis(); + break; + case "ascending": + double ascendingAnomaly = orbit + .trueAnomalyAtAN(MechPeste.getSpaceCenter().getTargetVessel().getOrbit()); + timeToNode = orbit.uTAtTrueAnomaly(ascendingAnomaly); + break; + case "descending": + double descendingAnomaly = orbit + .trueAnomalyAtDN(MechPeste.getSpaceCenter().getTargetVessel().getOrbit()); + timeToNode = orbit.uTAtTrueAnomaly(descendingAnomaly); + break; + } + currentManeuver.setUT(timeToNode); + // Print the maneuver node information + System.out.println("Maneuver Node updated:"); + System.out.println(" Time to node: " + currentManeuver.getTimeTo() + " s"); + } catch (Exception e) { + } + } + public void setupComponents() { // Setting-up components: - btnAdjustInc.addActionListener(this); - btnAdjustInc.setActionCommand(Modulos.AJUSTAR.get()); - btnAdjustInc.setEnabled(false); - btnAdjustInc.setMaximumSize(BTN_DIMENSION); - btnAdjustInc.setPreferredSize(BTN_DIMENSION); + btnAlignPlanes.addActionListener(this); + btnAlignPlanes.setMaximumSize(BTN_DIMENSION); + btnAlignPlanes.setPreferredSize(BTN_DIMENSION); + + btnRendezvous.addActionListener(this); + btnRendezvous.setMaximumSize(BTN_DIMENSION); + btnRendezvous.setPreferredSize(BTN_DIMENSION); btnApoapsis.addActionListener(this); - btnApoapsis.setActionCommand(Modulos.APOASTRO.get()); btnApoapsis.setMaximumSize(BTN_DIMENSION); btnApoapsis.setPreferredSize(BTN_DIMENSION); @@ -66,17 +134,14 @@ public void setupComponents() { btnBack.setPreferredSize(BTN_DIMENSION); btnExecute.addActionListener(this); - btnExecute.setActionCommand(Modulos.EXECUTAR.get()); btnExecute.setMaximumSize(BTN_DIMENSION); btnExecute.setPreferredSize(BTN_DIMENSION); btnLowerOrbit.addActionListener(this); - btnLowerOrbit.setActionCommand(Modulos.ORBITA_BAIXA.get()); btnLowerOrbit.setMaximumSize(BTN_DIMENSION); btnLowerOrbit.setPreferredSize(BTN_DIMENSION); btnPeriapsis.addActionListener(this); - btnPeriapsis.setActionCommand(Modulos.PERIASTRO.get()); btnPeriapsis.setMaximumSize(BTN_DIMENSION); btnPeriapsis.setPreferredSize(BTN_DIMENSION); } @@ -94,12 +159,11 @@ public void layoutComponents() { pnlExecuteManeuver.add(MainGui.createMarginComponent(10, 0)); pnlExecuteManeuver.add(btnExecute); - JPanel pnlAdjustInclination = new JPanel(); - pnlAdjustInclination.setLayout(new BoxLayout(pnlAdjustInclination, BoxLayout.X_AXIS)); - pnlAdjustInclination.setBorder(MARGIN_BORDER_10_PX_LR); - pnlAdjustInclination.add(lblAdjustInc); - pnlAdjustInclination.add(Box.createHorizontalGlue()); - pnlAdjustInclination.add(btnAdjustInc); + JPanel pnlAutoPosition = new JPanel(); + pnlAutoPosition.setLayout(new BoxLayout(pnlAutoPosition, BoxLayout.X_AXIS)); + pnlAutoPosition.setBorder(new TitledBorder("Auto posição:")); + pnlAutoPosition.add(btnAlignPlanes); + pnlAutoPosition.add(btnRendezvous); JPanel pnlCircularize = new JPanel(); pnlCircularize.setLayout(new BoxLayout(pnlCircularize, BoxLayout.X_AXIS)); @@ -115,7 +179,7 @@ public void layoutComponents() { JPanel pnlSetup = new JPanel(); pnlSetup.setLayout(new BoxLayout(pnlSetup, BoxLayout.Y_AXIS)); pnlSetup.add(pnlExecuteManeuver); - pnlSetup.add(pnlAdjustInclination); + pnlSetup.add(pnlAutoPosition); JPanel pnlOptions = new JPanel(); pnlOptions.setLayout(new BoxLayout(pnlOptions, BoxLayout.Y_AXIS)); @@ -157,9 +221,12 @@ public void actionPerformed(ActionEvent e) { if (e.getSource() == btnPeriapsis) { handleManeuverFunction(Modulos.PERIASTRO.get()); } - if (e.getSource() == btnAdjustInc) { + if (e.getSource() == btnAlignPlanes) { handleManeuverFunction(Modulos.AJUSTAR.get()); } + if (e.getSource() == btnRendezvous) { + handleManeuverFunction(Modulos.RENDEZVOUS.get()); + } if (e.getSource() == btnBack) { MainGui.backToTelemetry(e); } @@ -168,7 +235,7 @@ public void actionPerformed(ActionEvent e) { protected void handleManeuverFunction(String maneuverFunction) { Map commands = new HashMap<>(); commands.put(Modulos.MODULO.get(), Modulos.MODULE_MANEUVER.get()); - commands.put(Modulos.FUNCAO.get(), maneuverFunction); + commands.put(Modulos.FUNCAO.get(), maneuverFunction.toString()); commands.put(Modulos.AJUSTE_FINO.get(), String.valueOf(chkFineAdjusment.isSelected())); MechPeste.newInstance().startModule(commands); } From f46bb3b4da60d90cebde002da6f824fd6a0fe443 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Sat, 23 Sep 2023 00:30:47 -0300 Subject: [PATCH 10/19] -- First implementation of auto-docking --- src/com/pesterenan/Main.java | 114 +++++++++++++++++++++++++++++++++++ 1 file changed, 114 insertions(+) create mode 100644 src/com/pesterenan/Main.java diff --git a/src/com/pesterenan/Main.java b/src/com/pesterenan/Main.java new file mode 100644 index 0000000..bd0eb28 --- /dev/null +++ b/src/com/pesterenan/Main.java @@ -0,0 +1,114 @@ +package com.pesterenan; + +import java.io.IOException; + +import org.javatuples.Triplet; + +import com.pesterenan.utils.ControlePID; +import com.pesterenan.utils.Navigation; +import com.pesterenan.utils.Vector; +import krpc.client.Connection; +import krpc.client.RPCException; +import krpc.client.services.Drawing; +import krpc.client.services.SpaceCenter; +import krpc.client.services.Drawing.Line; +import krpc.client.services.SpaceCenter.DockingPort; +import krpc.client.services.SpaceCenter.ReferenceFrame; +import krpc.client.services.SpaceCenter.Vessel; + +public class Main { + private static Connection connection; + private static SpaceCenter spaceCenter; + private static Drawing drawing; + private static ControlePID ctrlRCS; + + public static void main(String[] args) { + try { + // Connect to the KSP instance + connection = Connection.newInstance(); + spaceCenter = SpaceCenter.newInstance(connection); + drawing = Drawing.newInstance(connection); + ctrlRCS = new ControlePID(); + ctrlRCS.adjustOutput(-1,1); + + Vessel minhaNave = spaceCenter.getActiveVessel(); + minhaNave.getAutoPilot().setTimeToPeak(new Vector(3, 3, 3).toTriplet()); + minhaNave.getAutoPilot().setDecelerationTime(new Vector(4, 4, 4).toTriplet()); + ReferenceFrame referenciaOrbital = minhaNave.getOrbit().getBody().getReferenceFrame(); + // Get the target vessel and its orbit + Vessel targetVessel = spaceCenter.getTargetVessel(); + + DockingPort minhaDockingPort = minhaNave.getParts().getDockingPorts().get(0); + DockingPort dockingPortAlvo = targetVessel.getParts().getDockingPorts().get(0); + + Line linhaDistancia = drawing.addLine(minhaDockingPort.position(referenciaOrbital), + dockingPortAlvo.position(referenciaOrbital), referenciaOrbital, true); + + + minhaNave.getAutoPilot().setReferenceFrame(referenciaOrbital); + minhaNave.getAutoPilot().setTargetRoll(0); + minhaNave.getAutoPilot().engage(); + double tempoDoLoop = 0; + while (tempoDoLoop < 10) { + // Fase 1: Apontar pra dockingport alvo de longe: + Vector posicaoDockingPortAlvo = new Vector(dockingPortAlvo.position(referenciaOrbital)); + Vector posicaoMinhaDockingPort = new Vector(minhaDockingPort.position(referenciaOrbital)); + + + // Calcular distancia: + Vector distanciaEntrePortas = posicaoDockingPortAlvo.subtract(posicaoMinhaDockingPort); + double distanciaEntrePortasEmMetros = distanciaEntrePortas.magnitude(); + + // Aproximar-se a 100 metros do alvo pra terminar a primeira fase: + double VEL_LIMIT = 2.0; // 1m/s² + Vector vetorVelocidadeMinhaNave = new Vector(minhaNave.velocity( + dockingPortAlvo.getReferenceFrame() + )); + double velocidadeAtualDaNave = vetorVelocidadeMinhaNave.magnitude(); + + // System.out.println(velocidadeAtualDaNave); + Vector direcaoMinhaNave = new Vector(minhaNave.direction(referenciaOrbital)); + Vector direcaoParaOAlvo = posicaoDockingPortAlvo.subtract(posicaoMinhaDockingPort).normalize(); + if (direcaoMinhaNave.heading() - direcaoParaOAlvo.heading() < 10) { + if (distanciaEntrePortasEmMetros > 50) { + minhaNave.getAutoPilot().setTargetDirection( + Vector.targetDirection(posicaoMinhaDockingPort, posicaoDockingPortAlvo).toTriplet()); + minhaNave.getControl().setForward( + (float) ctrlRCS.calcPID((velocidadeAtualDaNave / VEL_LIMIT) * 100, 100)); + } else { + fase2docking(minhaNave, dockingPortAlvo, referenciaOrbital); + } + } + + // Atualizando a linha entre as portas: + linhaDistancia.setStart(posicaoMinhaDockingPort.toTriplet()); + linhaDistancia.setEnd(posicaoDockingPortAlvo.toTriplet()); + + Thread.sleep(100); + tempoDoLoop += 0.1; + System.out.println(distanciaEntrePortasEmMetros + " metros"); + } + minhaNave.getAutoPilot().disengage(); + // // Close the connection when finished + connection.close(); + } catch (IOException | RPCException | InterruptedException e) { + e.printStackTrace(); + } + } + + public static void fase2docking(Vessel minhaNave, DockingPort dockingPortAlvo, ReferenceFrame referenciaOrbital) { + Vector direcaoApontandoDockingAlvo; + try { + direcaoApontandoDockingAlvo = new Vector(dockingPortAlvo.direction(referenciaOrbital)); + direcaoApontandoDockingAlvo.x = direcaoApontandoDockingAlvo.x * -1; + direcaoApontandoDockingAlvo.z = direcaoApontandoDockingAlvo.z * -1; + System.out.println(direcaoApontandoDockingAlvo); + + minhaNave.getAutoPilot().setTargetDirection( + direcaoApontandoDockingAlvo.toTriplet()); + } catch (RPCException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } +} \ No newline at end of file From 33d9b7393cec7bbb817ba6d7c4f99749832c2b11 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Fri, 29 Sep 2023 17:50:39 -0300 Subject: [PATCH 11/19] -- Managed to debug on Neovim --- .vscode/launch.json | 7 ++ src/com/pesterenan/Main.java | 19 ++--- src/com/pesterenan/MechPeste.java | 115 +++++++++++++++--------------- 3 files changed, 72 insertions(+), 69 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index beb5d53..c826b57 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,5 +1,12 @@ { "configurations": [ + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "com.pesterenan.Main", + "projectName": "MechPeste-Java" + }, { "type": "java", "name": "Launch MechPeste", diff --git a/src/com/pesterenan/Main.java b/src/com/pesterenan/Main.java index bd0eb28..0ca4c48 100644 --- a/src/com/pesterenan/Main.java +++ b/src/com/pesterenan/Main.java @@ -2,16 +2,14 @@ import java.io.IOException; -import org.javatuples.Triplet; - import com.pesterenan.utils.ControlePID; -import com.pesterenan.utils.Navigation; import com.pesterenan.utils.Vector; + import krpc.client.Connection; import krpc.client.RPCException; import krpc.client.services.Drawing; -import krpc.client.services.SpaceCenter; import krpc.client.services.Drawing.Line; +import krpc.client.services.SpaceCenter; import krpc.client.services.SpaceCenter.DockingPort; import krpc.client.services.SpaceCenter.ReferenceFrame; import krpc.client.services.SpaceCenter.Vessel; @@ -29,7 +27,7 @@ public static void main(String[] args) { spaceCenter = SpaceCenter.newInstance(connection); drawing = Drawing.newInstance(connection); ctrlRCS = new ControlePID(); - ctrlRCS.adjustOutput(-1,1); + ctrlRCS.adjustOutput(-1, 1); Vessel minhaNave = spaceCenter.getActiveVessel(); minhaNave.getAutoPilot().setTimeToPeak(new Vector(3, 3, 3).toTriplet()); @@ -42,8 +40,7 @@ public static void main(String[] args) { DockingPort dockingPortAlvo = targetVessel.getParts().getDockingPorts().get(0); Line linhaDistancia = drawing.addLine(minhaDockingPort.position(referenciaOrbital), - dockingPortAlvo.position(referenciaOrbital), referenciaOrbital, true); - + dockingPortAlvo.position(referenciaOrbital), referenciaOrbital, true); minhaNave.getAutoPilot().setReferenceFrame(referenciaOrbital); minhaNave.getAutoPilot().setTargetRoll(0); @@ -53,17 +50,15 @@ public static void main(String[] args) { // Fase 1: Apontar pra dockingport alvo de longe: Vector posicaoDockingPortAlvo = new Vector(dockingPortAlvo.position(referenciaOrbital)); Vector posicaoMinhaDockingPort = new Vector(minhaDockingPort.position(referenciaOrbital)); - // Calcular distancia: Vector distanciaEntrePortas = posicaoDockingPortAlvo.subtract(posicaoMinhaDockingPort); double distanciaEntrePortasEmMetros = distanciaEntrePortas.magnitude(); - + // Aproximar-se a 100 metros do alvo pra terminar a primeira fase: double VEL_LIMIT = 2.0; // 1m/s² Vector vetorVelocidadeMinhaNave = new Vector(minhaNave.velocity( - dockingPortAlvo.getReferenceFrame() - )); + dockingPortAlvo.getReferenceFrame())); double velocidadeAtualDaNave = vetorVelocidadeMinhaNave.magnitude(); // System.out.println(velocidadeAtualDaNave); @@ -111,4 +106,4 @@ public static void fase2docking(Vessel minhaNave, DockingPort dockingPortAlvo, R e.printStackTrace(); } } -} \ No newline at end of file +} diff --git a/src/com/pesterenan/MechPeste.java b/src/com/pesterenan/MechPeste.java index 71f917c..0367073 100644 --- a/src/com/pesterenan/MechPeste.java +++ b/src/com/pesterenan/MechPeste.java @@ -9,7 +9,8 @@ import java.util.Map; import java.util.stream.Collectors; -import javax.swing.*; +import javax.swing.DefaultListModel; +import javax.swing.ListModel; import com.pesterenan.model.ActiveVessel; import com.pesterenan.resources.Bundle; @@ -33,10 +34,6 @@ public class MechPeste { private static int currentVesselId = -1; private static ActiveVessel currentVessel = null; - private MechPeste() { - MainGui.newInstance(); - } - public static void main(String[] args) { MechPeste.newInstance().connectToKSP(); MechPeste.newInstance().checkActiveVessel(); @@ -92,32 +89,6 @@ public static ListModel getCurrentManeuvers() { return list; } - private static boolean filterVessels(Vessel vessel, String search) { - if (search == "all") { - return true; - } - double TEN_KILOMETERS = 10000.0; - try { - Vessel active = MechPeste.getSpaceCenter().getActiveVessel(); - if (vessel.getOrbit().getBody().getName().equals(active.getOrbit().getBody().getName())) { - final Vector activePos = new Vector(active.position(active.getSurfaceReferenceFrame())); - final Vector vesselPos = new Vector(vessel.position(active.getSurfaceReferenceFrame())); - final double distance = Vector.distance(activePos, vesselPos); - switch (search) { - case "closest": - if (distance < TEN_KILOMETERS) { - return true; - } - break; - case "samebody": - return true; - } - } - } catch (RPCException ignored) { - } - return false; - } - public static String getVesselInfo(int selectedIndex) { try { Vessel naveAtual = spaceCenter.getVessels().stream().filter(v -> v.hashCode() == selectedIndex).findFirst() @@ -143,35 +114,42 @@ public static void changeToVessel(int selectedIndex) { } } - public KRPC.GameScene getCurrentGameScene() throws RPCException { - return krpc.getCurrentGameScene(); + public static void cancelControl(ActionEvent e) { + currentVessel.cancelControl(); } - private void checkActiveVessel() { - while (getConnection() != null) { - try { - if (!MechPeste.newInstance().getCurrentGameScene().equals(KRPC.GameScene.FLIGHT)) { - Thread.sleep(100); - return; - } - int activeVesselId = spaceCenter.getActiveVessel().hashCode(); - // If the current active vessel changes, create a new connection - if (currentVesselId != activeVesselId) { - currentVessel = new ActiveVessel(); - currentVesselId = currentVessel.getCurrentVesselId(); - } - if (currentVesselId != -1) { - currentVessel.recordTelemetryData(); - if (currentVessel.hasModuleRunning()) { - setStatusMessage(currentVessel.getCurrentStatus()); - } - FunctionsAndTelemetryJPanel.updateTelemetry(currentVessel.getTelemetryData()); - CreateManeuverJPanel.updatePanel(getCurrentManeuvers()); + private static boolean filterVessels(Vessel vessel, String search) { + if (search == "all") { + return true; + } + double TEN_KILOMETERS = 10000.0; + try { + Vessel active = MechPeste.getSpaceCenter().getActiveVessel(); + if (vessel.getOrbit().getBody().getName().equals(active.getOrbit().getBody().getName())) { + final Vector activePos = new Vector(active.position(active.getSurfaceReferenceFrame())); + final Vector vesselPos = new Vector(vessel.position(active.getSurfaceReferenceFrame())); + final double distance = Vector.distance(activePos, vesselPos); + switch (search) { + case "closest": + if (distance < TEN_KILOMETERS) { + return true; + } + break; + case "samebody": + return true; } - Thread.sleep(100); - } catch (RPCException | InterruptedException ignored) { } + } catch (RPCException ignored) { } + return false; + } + + private MechPeste() { + MainGui.newInstance(); + } + + public KRPC.GameScene getCurrentGameScene() throws RPCException { + return krpc.getCurrentGameScene(); } public void startModule(Map commands) { @@ -205,7 +183,30 @@ public void checkConnection() { } } - public static void cancelControl(ActionEvent e) { - currentVessel.cancelControl(); + private void checkActiveVessel() { + while (getConnection() != null) { + try { + if (!MechPeste.newInstance().getCurrentGameScene().equals(KRPC.GameScene.FLIGHT)) { + Thread.sleep(100); + return; + } + int activeVesselId = spaceCenter.getActiveVessel().hashCode(); + // If the current active vessel changes, create a new connection + if (currentVesselId != activeVesselId) { + currentVessel = new ActiveVessel(); + currentVesselId = currentVessel.getCurrentVesselId(); + } + if (currentVesselId != -1) { + currentVessel.recordTelemetryData(); + if (currentVessel.hasModuleRunning()) { + setStatusMessage(currentVessel.getCurrentStatus()); + } + FunctionsAndTelemetryJPanel.updateTelemetry(currentVessel.getTelemetryData()); + CreateManeuverJPanel.updatePanel(getCurrentManeuvers()); + } + Thread.sleep(100); + } catch (RPCException | InterruptedException ignored) { + } + } } } From 672dab2d2e2a6e51796ed97529da8b4ad6614fa2 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Wed, 4 Oct 2023 19:27:01 -0300 Subject: [PATCH 12/19] -- Changed how ControlePID works --- src/com/pesterenan/Main.java | 236 +++++++++++++----- .../controllers/LandingController.java | 61 ++--- .../controllers/LiftoffController.java | 15 +- .../controllers/ManeuverController.java | 208 +++++++++------ .../controllers/RoverController.java | 10 +- src/com/pesterenan/utils/ControlePID.java | 88 +++++-- src/com/pesterenan/utils/Vector.java | 15 +- .../views/CreateManeuverJPanel.java | 2 +- 8 files changed, 419 insertions(+), 216 deletions(-) diff --git a/src/com/pesterenan/Main.java b/src/com/pesterenan/Main.java index 0ca4c48..76884c4 100644 --- a/src/com/pesterenan/Main.java +++ b/src/com/pesterenan/Main.java @@ -2,6 +2,8 @@ import java.io.IOException; +import org.javatuples.Triplet; + import com.pesterenan.utils.ControlePID; import com.pesterenan.utils.Vector; @@ -10,80 +12,150 @@ import krpc.client.services.Drawing; import krpc.client.services.Drawing.Line; import krpc.client.services.SpaceCenter; +import krpc.client.services.SpaceCenter.Control; import krpc.client.services.SpaceCenter.DockingPort; import krpc.client.services.SpaceCenter.ReferenceFrame; +import krpc.client.services.SpaceCenter.SASMode; import krpc.client.services.SpaceCenter.Vessel; public class Main { private static Connection connection; private static SpaceCenter spaceCenter; private static Drawing drawing; - private static ControlePID ctrlRCS; - - public static void main(String[] args) { + private static ControlePID speedCtrl; + private static ControlePID distCtrl; + private static ControlePID RCSForwardsCtrl; + private static ControlePID RCSSidewaysCtrl; + private static ControlePID RCSUpwardsCtrl; + private static Control control; + + private static Vessel activeVessel; + private static Vessel targetVessel; + + private static ReferenceFrame orbitalRefVessel; + private static ReferenceFrame vesselRefFrame; + private static ReferenceFrame orbitalRefBody; + private static final double SPEED_LIMIT = 5.0; + private static final double DISTANCE_LIMIT = 50.0; + private static Line distanceLine; + private static Line distLineXAxis; + private static Line distLineYAxis; + private static Line distLineZAxis; + + private static void initializeParameters() { try { - // Connect to the KSP instance connection = Connection.newInstance(); spaceCenter = SpaceCenter.newInstance(connection); drawing = Drawing.newInstance(connection); - ctrlRCS = new ControlePID(); - ctrlRCS.adjustOutput(-1, 1); - - Vessel minhaNave = spaceCenter.getActiveVessel(); - minhaNave.getAutoPilot().setTimeToPeak(new Vector(3, 3, 3).toTriplet()); - minhaNave.getAutoPilot().setDecelerationTime(new Vector(4, 4, 4).toTriplet()); - ReferenceFrame referenciaOrbital = minhaNave.getOrbit().getBody().getReferenceFrame(); - // Get the target vessel and its orbit - Vessel targetVessel = spaceCenter.getTargetVessel(); - - DockingPort minhaDockingPort = minhaNave.getParts().getDockingPorts().get(0); - DockingPort dockingPortAlvo = targetVessel.getParts().getDockingPorts().get(0); - - Line linhaDistancia = drawing.addLine(minhaDockingPort.position(referenciaOrbital), - dockingPortAlvo.position(referenciaOrbital), referenciaOrbital, true); - - minhaNave.getAutoPilot().setReferenceFrame(referenciaOrbital); - minhaNave.getAutoPilot().setTargetRoll(0); - minhaNave.getAutoPilot().engage(); - double tempoDoLoop = 0; - while (tempoDoLoop < 10) { - // Fase 1: Apontar pra dockingport alvo de longe: - Vector posicaoDockingPortAlvo = new Vector(dockingPortAlvo.position(referenciaOrbital)); - Vector posicaoMinhaDockingPort = new Vector(minhaDockingPort.position(referenciaOrbital)); + activeVessel = spaceCenter.getActiveVessel(); + targetVessel = spaceCenter.getTargetVessel(); + vesselRefFrame = activeVessel.getReferenceFrame(); + orbitalRefVessel = activeVessel.getOrbitalReferenceFrame(); + orbitalRefBody = activeVessel.getOrbit().getBody().getReferenceFrame(); + + speedCtrl = new ControlePID(spaceCenter, 50); + speedCtrl.setPIDValues(0.5, 0.001, 0.01); + distCtrl = new ControlePID(spaceCenter, 50); + distCtrl.setPIDValues(1, 0.001, 0.1); + RCSForwardsCtrl = new ControlePID(spaceCenter, 50); + RCSSidewaysCtrl = new ControlePID(spaceCenter, 50); + RCSUpwardsCtrl = new ControlePID(spaceCenter, 50); + + } catch (IOException | RPCException e) { + e.printStackTrace(); + } + } + + public static void main(String[] args) { + try { + // Initialize parameters for the script, connection and setup: + initializeParameters(); + + DockingPort myDockingPort = activeVessel.getParts().getDockingPorts().get(0); + DockingPort targetDockingPort = targetVessel.getParts().getDockingPorts().get(0); + + // Fase 1: Apontar pra dockingport alvo de longe: + Vector positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); + Vector positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); + createLines(positionMyDockingPort, positionTargetDockingPort); + + // Setting up the control + control = activeVessel.getControl(); + control.setSAS(true); + control.setSASMode(SASMode.STABILITY_ASSIST); + + double loopTimeInSeconds = 0; + while (loopTimeInSeconds < 30) { // Calcular distancia: - Vector distanciaEntrePortas = posicaoDockingPortAlvo.subtract(posicaoMinhaDockingPort); - double distanciaEntrePortasEmMetros = distanciaEntrePortas.magnitude(); - - // Aproximar-se a 100 metros do alvo pra terminar a primeira fase: - double VEL_LIMIT = 2.0; // 1m/s² - Vector vetorVelocidadeMinhaNave = new Vector(minhaNave.velocity( - dockingPortAlvo.getReferenceFrame())); - double velocidadeAtualDaNave = vetorVelocidadeMinhaNave.magnitude(); - - // System.out.println(velocidadeAtualDaNave); - Vector direcaoMinhaNave = new Vector(minhaNave.direction(referenciaOrbital)); - Vector direcaoParaOAlvo = posicaoDockingPortAlvo.subtract(posicaoMinhaDockingPort).normalize(); - if (direcaoMinhaNave.heading() - direcaoParaOAlvo.heading() < 10) { - if (distanciaEntrePortasEmMetros > 50) { - minhaNave.getAutoPilot().setTargetDirection( - Vector.targetDirection(posicaoMinhaDockingPort, posicaoDockingPortAlvo).toTriplet()); - minhaNave.getControl().setForward( - (float) ctrlRCS.calcPID((velocidadeAtualDaNave / VEL_LIMIT) * 100, 100)); + double distanceBetweenPortsInMeters = positionTargetDockingPort.subtract(positionMyDockingPort) + .magnitude(); + double currentRelativeVelocity = new Vector(activeVessel.velocity( + targetDockingPort.getReferenceFrame())).magnitude(); + positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); + positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); + + double speedOutput = 0; + double distanceOutput = 0; + + setDirection(distanceBetweenPortsInMeters); + if (distanceBetweenPortsInMeters > DISTANCE_LIMIT) { + speedOutput = speedCtrl.calculate(currentRelativeVelocity, SPEED_LIMIT); + distanceOutput = distCtrl.calculate(-distanceBetweenPortsInMeters, + DISTANCE_LIMIT); + System.out.println("Speed: " + speedOutput + " Distance: " + distanceOutput + + " Calc: " + + (speedOutput * distanceOutput)); + control.setForward( + (float) (speedOutput * distanceOutput)); + } else { + if (currentRelativeVelocity <= 3.0) { + speedOutput = speedCtrl.calculate(currentRelativeVelocity, 0.1); + control.setForward((float) (speedOutput)); + if (currentRelativeVelocity < 0.3) { + break; + } } else { - fase2docking(minhaNave, dockingPortAlvo, referenciaOrbital); + speedOutput = speedCtrl.calculate(currentRelativeVelocity, 2); + control.setForward((float) (speedOutput * 0.5)); } + System.out.println("Vel: " + currentRelativeVelocity + " PID: " + + speedOutput); } + updateLines(positionMyDockingPort, positionTargetDockingPort); + Thread.sleep(50); + loopTimeInSeconds += 0.05; + } - // Atualizando a linha entre as portas: - linhaDistancia.setStart(posicaoMinhaDockingPort.toTriplet()); - linhaDistancia.setEnd(posicaoDockingPortAlvo.toTriplet()); - - Thread.sleep(100); - tempoDoLoop += 0.1; - System.out.println(distanciaEntrePortasEmMetros + " metros"); + // Triplet direcaoTransformada = spaceCenter + // .transformDirection(targetDockingPort.direction(orbitalRefVessel), + // orbitalRefVessel, + // orbitalRefVessel); + + // SEGUNDA PARTE APONTAR PRO LADO CONTRARIO: + Vector direcaoContrariaDockingPortAlvo = new Vector(targetDockingPort.direction(orbitalRefVessel)) + .multiply(-1); + control.setSAS(false); + control.setRCS(false); + activeVessel.getAutoPilot().engage(); + activeVessel.getAutoPilot().setReferenceFrame(orbitalRefVessel); + activeVessel.getAutoPilot().setTargetDirection(direcaoContrariaDockingPortAlvo.toTriplet()); + loopTimeInSeconds = 0; + + while (loopTimeInSeconds < 10) { + System.out.println("apontando"); + activeVessel.getAutoPilot().wait_(); + if (activeVessel.getAutoPilot().getError() < 1) { + break; + } + Thread.sleep(50); + loopTimeInSeconds += 0.05; } - minhaNave.getAutoPilot().disengage(); + control.setSAS(true); + control.setSASMode(SASMode.STABILITY_ASSIST); + activeVessel.getAutoPilot().disengage(); + + Thread.sleep(1000); // // Close the connection when finished connection.close(); } catch (IOException | RPCException | InterruptedException e) { @@ -91,19 +163,53 @@ public static void main(String[] args) { } } - public static void fase2docking(Vessel minhaNave, DockingPort dockingPortAlvo, ReferenceFrame referenciaOrbital) { - Vector direcaoApontandoDockingAlvo; + private static void setDirection(double distance) { + try { + if (distance > DISTANCE_LIMIT) { + control.setSASMode(SASMode.TARGET); + } else { + control.setSASMode(SASMode.STABILITY_ASSIST); + } + } catch (RPCException e) { + } + } + + private static void createLines(Vector start, Vector end) { try { - direcaoApontandoDockingAlvo = new Vector(dockingPortAlvo.direction(referenciaOrbital)); - direcaoApontandoDockingAlvo.x = direcaoApontandoDockingAlvo.x * -1; - direcaoApontandoDockingAlvo.z = direcaoApontandoDockingAlvo.z * -1; - System.out.println(direcaoApontandoDockingAlvo); + distanceLine = drawing.addLine(start.toTriplet(), + end.toTriplet(), vesselRefFrame, true); + distLineXAxis = drawing.addLine(start.toTriplet(), + new Vector(end.x, 0.0, 0.0).toTriplet(), + vesselRefFrame, true); + distLineYAxis = drawing.addLine(start.toTriplet(), + new Vector(end.x, end.y, 0.0).toTriplet(), + vesselRefFrame, true); + distLineZAxis = drawing.addLine(start.toTriplet(), + end.toTriplet(), + vesselRefFrame, true); + distanceLine.setThickness(0.5f); + distLineXAxis.setThickness(0.25f); + distLineYAxis.setThickness(0.25f); + distLineZAxis.setThickness(0.25f); + distLineXAxis.setColor(new Vector(1.0, 0.0, 0.0).toTriplet()); + distLineYAxis.setColor(new Vector(0.0, 1.0, 0.0).toTriplet()); + distLineZAxis.setColor(new Vector(0.0, 0.0, 1.0).toTriplet()); + } catch (RPCException e) { + } + } - minhaNave.getAutoPilot().setTargetDirection( - direcaoApontandoDockingAlvo.toTriplet()); + private static void updateLines(Vector start, Vector end) { + // Updating drawing lines: + try { + distanceLine.setStart(start.toTriplet()); + distanceLine.setEnd(end.toTriplet()); + distLineXAxis.setStart(start.toTriplet()); + distLineXAxis.setEnd(new Vector(end.x, 0.0, 0.0).toTriplet()); + distLineYAxis.setStart(distLineXAxis.getEnd()); + distLineYAxis.setEnd(new Vector(end.x, end.y, 0.0).toTriplet()); + distLineZAxis.setStart(distLineYAxis.getEnd()); + distLineZAxis.setEnd(end.toTriplet()); } catch (RPCException e) { - // TODO Auto-generated catch block - e.printStackTrace(); } } } diff --git a/src/com/pesterenan/controllers/LandingController.java b/src/com/pesterenan/controllers/LandingController.java index 4cd37eb..81761bb 100644 --- a/src/com/pesterenan/controllers/LandingController.java +++ b/src/com/pesterenan/controllers/LandingController.java @@ -1,26 +1,29 @@ package com.pesterenan.controllers; +import static com.pesterenan.MechPeste.getSpaceCenter; + +import java.util.Map; + import com.pesterenan.resources.Bundle; import com.pesterenan.utils.ControlePID; import com.pesterenan.utils.Modulos; import com.pesterenan.utils.Navigation; import com.pesterenan.utils.Utilities; + import krpc.client.RPCException; import krpc.client.StreamException; import krpc.client.services.SpaceCenter.VesselSituation; -import java.util.Map; - public class LandingController extends Controller { public static final double MAX_VELOCITY = 5; private static final double velP = 0.025; private static final double velI = 0.001; private static final double velD = 0.01; - private final ControlePID altitudeCtrl = new ControlePID(); - private final ControlePID velocityCtrl = new ControlePID(); + private ControlePID altitudeCtrl; + private ControlePID velocityCtrl; private Navigation navigation; - private final int HUNDRED_PERCENT = 100; + private final int HUNDRED_PERCENT = 200; private double hoverAltitude; private boolean hoveringMode; private boolean hoverAfterApproximation; @@ -37,8 +40,10 @@ public LandingController(Map commands) { } private void initializeParameters() { - altitudeCtrl.adjustOutput(0, 1); - velocityCtrl.adjustOutput(0, 1); + altitudeCtrl = new ControlePID(getSpaceCenter(), 25); + velocityCtrl = new ControlePID(getSpaceCenter(), 25); + altitudeCtrl.setOutput(0, 1); + velocityCtrl.setOutput(0, 1); } @Override @@ -75,7 +80,7 @@ private void hoverArea() { changeControlMode(); } catch (RPCException | StreamException ignored) { } - Thread.sleep(50); + Thread.sleep(25); } } catch (InterruptedException | RPCException ignored) { // disengageAfterException(Bundle.getString("status_liftoff_abort")); @@ -103,7 +108,7 @@ private void autoLanding() { } getNaveAtual().getControl().setBrakes(true); changeControlMode(); - Thread.sleep(100); + Thread.sleep(25); } } catch (RPCException | StreamException | InterruptedException e) { setCurrentStatus(Bundle.getString("status_ready")); @@ -128,16 +133,16 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt } break; case APPROACHING: - altitudeCtrl.adjustOutput(0, 1); - velocityCtrl.adjustOutput(0, 1); + altitudeCtrl.setOutput(0, 1); + velocityCtrl.setOutput(0, 1); double currentVelocity = calculateCurrentVelocityMagnitude(); double zeroVelocity = calculateZeroVelocityMagnitude(); double landingDistanceThreshold = Math.max(hoverAltitude, getMaxAcel(maxTWR) * 3); double threshold = Utilities.clamp( ((currentVelocity + zeroVelocity) - landingDistanceThreshold) / landingDistanceThreshold, 0, 1); - altPID = altitudeCtrl.calcPID(currentVelocity / zeroVelocity * HUNDRED_PERCENT, HUNDRED_PERCENT); - velPID = velocityCtrl.calcPID(velVertical.get(), -Utilities.clamp(altitudeSup.get() * 0.1, 1, 10)); + altPID = altitudeCtrl.calculate(currentVelocity / zeroVelocity * HUNDRED_PERCENT, HUNDRED_PERCENT); + velPID = velocityCtrl.calculate(velVertical.get(), -Utilities.clamp(altitudeSup.get() * 0.1, 1, 10)); throttle(Utilities.linearInterpolation(velPID, altPID, threshold)); navigation.aimForLanding(); if (threshold < 0.25 || altitudeSup.get() < landingDistanceThreshold) { @@ -153,10 +158,10 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt setCurrentStatus("Se aproximando do momento do pouso..."); break; case GOING_UP: - altitudeCtrl.adjustOutput(-0.5, 0.5); - velocityCtrl.adjustOutput(-0.5, 0.5); - altPID = altitudeCtrl.calcPID(altitudeErrorPercentage, HUNDRED_PERCENT); - velPID = velocityCtrl.calcPID(velVertical.get(), MAX_VELOCITY); + altitudeCtrl.setOutput(-0.5, 0.5); + velocityCtrl.setOutput(-0.5, 0.5); + altPID = altitudeCtrl.calculate(altitudeErrorPercentage, HUNDRED_PERCENT); + velPID = velocityCtrl.calculate(velVertical.get(), MAX_VELOCITY); throttle(altPID + velPID); navigation.aimAtRadialOut(); setCurrentStatus("Subindo altitude..."); @@ -174,10 +179,10 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt hasTheVesselLanded(); break; case HOVERING: - altitudeCtrl.adjustOutput(-0.5, 0.5); - velocityCtrl.adjustOutput(-0.5, 0.5); - altPID = altitudeCtrl.calcPID(altitudeErrorPercentage, HUNDRED_PERCENT); - velPID = velocityCtrl.calcPID(velVertical.get(), 0); + altitudeCtrl.setOutput(-0.5, 0.5); + velocityCtrl.setOutput(-0.5, 0.5); + altPID = altitudeCtrl.calculate(altitudeErrorPercentage, HUNDRED_PERCENT); + velPID = velocityCtrl.calculate(velVertical.get(), 0); throttle(altPID + velPID); navigation.aimAtRadialOut(); setCurrentStatus("Sobrevoando area..."); @@ -187,8 +192,8 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt private void controlThrottleByMatchingVerticalVelocity(double velocityToMatch) throws RPCException, StreamException { - velocityCtrl.adjustOutput(0, 1); - throttle(velocityCtrl.calcPID(velVertical.get(), velocityToMatch)); + velocityCtrl.setOutput(0, 1); + throttle(velocityCtrl.calculate(velVertical.get(), velocityToMatch)); } private void deOrbitShip() throws RPCException, StreamException, InterruptedException { @@ -202,15 +207,15 @@ private void deOrbitShip() throws RPCException, StreamException, InterruptedExce navigation.aimForLanding(); setCurrentStatus(Bundle.getString("status_orienting_ship")); ap.wait_(); - Thread.sleep(100); + Thread.sleep(25); } double targetPeriapsis = currentBody.getAtmosphereDepth(); targetPeriapsis = targetPeriapsis > 0 ? targetPeriapsis / 2 : -currentBody.getEquatorialRadius() / 2; while (periastro.get() > -apoastro.get()) { navigation.aimForLanding(); - throttle(altitudeCtrl.calcPID(targetPeriapsis, periastro.get())); + throttle(altitudeCtrl.calculate(targetPeriapsis, periastro.get())); setCurrentStatus(Bundle.getString("status_lowering_periapsis")); - Thread.sleep(100); + Thread.sleep(25); } getNaveAtual().getControl().setRCS(false); throttle(0.0f); @@ -222,8 +227,8 @@ private void deOrbitShip() throws RPCException, StreamException, InterruptedExce */ private void adjustPIDbyTWR() throws RPCException, StreamException { double currentTWR = Math.min(getTWR(), maxTWR); - velocityCtrl.adjustPID(currentTWR * velP, velI, velD); - altitudeCtrl.adjustPID(currentTWR * velP, velI, velD); + velocityCtrl.setPIDValues(currentTWR * velP, velI, velD); + altitudeCtrl.setPIDValues(currentTWR * velP, velI, velD); } private double calculateCurrentVelocityMagnitude() throws RPCException, StreamException { diff --git a/src/com/pesterenan/controllers/LiftoffController.java b/src/com/pesterenan/controllers/LiftoffController.java index 4f5259e..b81b0d8 100644 --- a/src/com/pesterenan/controllers/LiftoffController.java +++ b/src/com/pesterenan/controllers/LiftoffController.java @@ -1,5 +1,7 @@ package com.pesterenan.controllers; +import static com.pesterenan.MechPeste.getSpaceCenter; + import com.pesterenan.MechPeste; import com.pesterenan.resources.Bundle; import com.pesterenan.utils.ControlePID; @@ -18,7 +20,7 @@ public class LiftoffController extends Controller { private static final float PITCH_UP = 90; - private final ControlePID thrControl = new ControlePID(); + private ControlePID thrControl; private float currentPitch; private float finalApoapsisAlt; private float heading; @@ -45,7 +47,8 @@ private void initializeParameters() { setGravityCurveModel(commands.get(Modulos.INCLINACAO.get())); willOpenPanelsAndAntenna = Boolean.parseBoolean(commands.get(Modulos.ABRIR_PAINEIS.get())); willDecoupleStages = Boolean.parseBoolean(commands.get(Modulos.USAR_ESTAGIOS.get())); - thrControl.adjustOutput(0.0, 1.0); + thrControl = new ControlePID(getSpaceCenter(), 25); + thrControl.setOutput(0.0, 1.0); } @Override @@ -79,7 +82,7 @@ private void gravityCurve() throws RPCException, StreamException, InterruptedExc currentPitch = (float) (calculateCurrentPitch(altitudeProgress)); double currentMaxTWR = calculateTWRBasedOnPressure(currentPitch); ap.setTargetPitch(currentPitch); - throttle(Math.min(thrControl.calcPID(apoastro.get() / getFinalApoapsis() * 1000, 1000), + throttle(Math.min(thrControl.calculate(apoastro.get() / getFinalApoapsis() * 1000, 1000), getMaxThrottleForTWR(currentMaxTWR))); if (willDecoupleStages && isCurrentStageWithoutFuel()) { @@ -87,7 +90,7 @@ private void gravityCurve() throws RPCException, StreamException, InterruptedExc } setCurrentStatus(String.format(Bundle.getString("status_liftoff_inclination") + " %.1f", currentPitch)); - Thread.sleep(250); + Thread.sleep(25); if (Thread.interrupted()) { throw new InterruptedException(); } @@ -111,8 +114,8 @@ private void finalizeCurve() throws RPCException, StreamException, InterruptedEx throw new InterruptedException(); } navigation.aimAtPrograde(); - throttle(thrControl.calcPID(apoastro.get() / getFinalApoapsis() * 1000, 1000)); - Thread.sleep(100); + throttle(thrControl.calculate(apoastro.get() / getFinalApoapsis() * 1000, 1000)); + Thread.sleep(25); } throttle(0.0f); if (willDecoupleStages) { diff --git a/src/com/pesterenan/controllers/ManeuverController.java b/src/com/pesterenan/controllers/ManeuverController.java index 98449a5..10f4f96 100644 --- a/src/com/pesterenan/controllers/ManeuverController.java +++ b/src/com/pesterenan/controllers/ManeuverController.java @@ -1,5 +1,13 @@ package com.pesterenan.controllers; +import static com.pesterenan.MechPeste.getConnection; +import static com.pesterenan.MechPeste.getSpaceCenter; + +import java.util.List; +import java.util.Map; + +import org.javatuples.Triplet; + import com.pesterenan.resources.Bundle; import com.pesterenan.utils.Attributes; import com.pesterenan.utils.ControlePID; @@ -18,19 +26,12 @@ import krpc.client.services.SpaceCenter.ReferenceFrame; import krpc.client.services.SpaceCenter.Vessel; import krpc.client.services.SpaceCenter.VesselSituation; -import org.javatuples.Triplet; - -import java.util.List; -import java.util.Map; - -import static com.pesterenan.MechPeste.getConnection; -import static com.pesterenan.MechPeste.getSpaceCenter; public class ManeuverController extends Controller { public final static float CONST_GRAV = 9.81f; - private final ControlePID ctrlRCS = new ControlePID(); - private final ControlePID ctrlManeuver = new ControlePID(); + private ControlePID ctrlRCS; + private ControlePID ctrlManeuver; private Navigation navigation; private boolean fineAdjustment; private double lowOrbitAltitude; @@ -43,7 +44,10 @@ public ManeuverController(Map commands) { } private void initializeParameters() { - ctrlRCS.adjustOutput(0.5, 1.0); + ctrlRCS = new ControlePID(getSpaceCenter(), 25); + ctrlManeuver = new ControlePID(getSpaceCenter(), 25); + ctrlManeuver.setPIDValues(1, 0.001, 0.1); + ctrlRCS.setOutput(0.5, 1.0); fineAdjustment = canFineAdjust(commands.get(Modulos.AJUSTE_FINO.get())); try { lowOrbitAltitude = new Attributes().getLowOrbitAltitude(currentBody.getName()); @@ -157,10 +161,10 @@ public void matchOrbitApoapsis() { break; } double dvPrograde = maneuver.getPrograde(); - double ctrlOutput = ctrlManeuver.calcPID(currentDeltaApo, 0); + double ctrlOutput = ctrlManeuver.calculate(currentDeltaApo, 0); maneuver.setPrograde(dvPrograde - (ctrlOutput)); - Thread.sleep(50); + Thread.sleep(25); } } catch (Exception e) { setCurrentStatus("Não foi possivel ajustar a inclinação"); @@ -205,10 +209,11 @@ private void alignPlanesWithTargetVessel() { RunManeuverJPanel.positionManeuverAt(closestIsAN ? "ascending" : "descending"); double currentInclination = Math .toDegrees(currentManeuver.getOrbit().relativeInclination(targetVesselOrbit)); + ctrlManeuver.setTimeSample(25); while (currentInclination > 0.05) { currentInclination = Math .toDegrees(currentManeuver.getOrbit().relativeInclination(targetVesselOrbit)); - double ctrlOutput = ctrlManeuver.calcPID(currentInclination * 100, 0); + double ctrlOutput = ctrlManeuver.calculate(currentInclination * 100, 0); currentManeuver.setNormal(currentManeuver.getNormal() + (closestIsAN ? ctrlOutput : -ctrlOutput)); Thread.sleep(25); } @@ -219,9 +224,8 @@ private void alignPlanesWithTargetVessel() { private void rendezvousWithTargetVessel() { try { - Orbit targetVesselOrbit = getSpaceCenter().getTargetVessel().getOrbit(); boolean hasManeuverNodes = getNaveAtual().getControl().getNodes().size() > 0; - java.util.List currentManeuvers = getNaveAtual().getControl().getNodes(); + List currentManeuvers = getNaveAtual().getControl().getNodes(); Node lastManeuverNode; double lastManeuverNodeUT = 60; if (hasManeuverNodes) { @@ -234,72 +238,99 @@ private void rendezvousWithTargetVessel() { } currentManeuvers = getNaveAtual().getControl().getNodes(); lastManeuverNode = currentManeuvers.get(currentManeuvers.size() - 1); - double targetAP = targetVesselOrbit.getApoapsis(); - double targetPE = targetVesselOrbit.getPeriapsis(); - double maneuverAP = lastManeuverNode.getOrbit().getApoapsis(); - double maneuverPE = lastManeuverNode.getOrbit().getPeriapsis(); - double maneuverUT = lastManeuverNode.getUT(); - ctrlManeuver.adjustPID(0.25, 0.0, 0.01); - ctrlManeuver.adjustOutput(-100, 100); - if (targetAP < maneuverPE) { - while (Math.floor(targetAP) != Math.floor(maneuverPE)) { - lastManeuverNode.setPrograde( - lastManeuverNode.getPrograde() - + ctrlManeuver.calcPID(maneuverPE / targetAP * 1000, 1000)); - maneuverPE = lastManeuverNode.getOrbit().getPeriapsis(); - Thread.sleep(25); - } - } - if (targetPE > maneuverAP) { - while (Math.floor(targetPE) != Math.floor(maneuverAP)) { - lastManeuverNode.setPrograde( - lastManeuverNode.getPrograde() - + ctrlManeuver.calcPID(maneuverAP / targetPE * 1000, 1000)); - maneuverAP = lastManeuverNode.getOrbit().getApoapsis(); - Thread.sleep(25); + + Orbit activeVesselOrbit = getNaveAtual().getOrbit(); + Orbit targetVesselOrbit = getSpaceCenter().getTargetVessel().getOrbit(); + ReferenceFrame currentBodyRefFrame = activeVesselOrbit.getBody().getNonRotatingReferenceFrame(); + + double angularDiff = 10; + while (angularDiff >= 0.005) { + double maneuverUT = lastManeuverNode.getUT(); + double targetOrbitPosition = new Vector( + targetVesselOrbit.positionAt(maneuverUT, currentBodyRefFrame)) + .magnitude(); + double maneuverAP = lastManeuverNode.getOrbit().getApoapsis(); + double maneuverPE = lastManeuverNode.getOrbit().getPeriapsis(); + ctrlManeuver.setPIDValues(0.25, 0.0, 0.01); + ctrlManeuver.setOutput(-100, 100); + + if (targetOrbitPosition < maneuverPE) { + while (Math.floor(targetOrbitPosition) != Math.floor(maneuverPE)) { + lastManeuverNode.setPrograde( + lastManeuverNode.getPrograde() + + ctrlManeuver.calculate(maneuverPE / targetOrbitPosition * 1000, 1000)); + maneuverPE = lastManeuverNode.getOrbit().getPeriapsis(); + Thread.sleep(25); + } } - } - double mu = currentBody.getGravitationalParameter(); - double time = 1000; - - double hohmannTransferDistance = lastManeuverNode.getOrbit().getSemiMajorAxis(); - double timeOfFlight = Math.PI * Math.sqrt(Math.pow(hohmannTransferDistance, 3) / mu); - double angle = getNaveAtual().getOrbit().getMeanAnomalyAtEpoch(); - double omegaInterceptor = Math - .sqrt(mu / Math.pow(getNaveAtual().getOrbit().radiusAt(getSpaceCenter().getUT()), 3)); // rad/s - double omegaTarget = Math.sqrt(mu / Math.pow(targetVesselOrbit.radiusAt(getSpaceCenter().getUT()), 3)); // rad/s - // double leadAngle = omegaTarget * timeOfFlight; // rad - double leadAngle = targetVesselOrbit.getMeanAnomalyAtEpoch(); // rad - double phaseAngle = Math.PI - leadAngle; // rad - double calcAngle = (phaseAngle - angle); - calcAngle = calcAngle < 0 ? calcAngle + (Math.PI * 2) : calcAngle; - double waitTime = calcAngle / (omegaTarget - omegaInterceptor); - time = waitTime; - - lastManeuverNode.setUT(getSpaceCenter().getUT() + time); - ctrlManeuver.adjustOutput(-100, 100); - ctrlManeuver.adjustPID(0.05, 0.1, 0.01); - double closestApproach = lastManeuverNode.getOrbit().distanceAtClosestApproach(targetVesselOrbit); - System.out.println(closestApproach); - System.out.println("Ajustando tempo de Rendezvous..."); - while (Math.round(closestApproach) > 100) { - if (closestApproach < 100000) { - ctrlManeuver.adjustOutput(-10, 10); - } else if (closestApproach < 10000) { - ctrlManeuver.adjustOutput(-1, 1); - } else { - ctrlManeuver.adjustOutput(-100, 100); + if (targetOrbitPosition > maneuverAP) { + while (Math.floor(targetOrbitPosition) != Math.floor(maneuverAP)) { + lastManeuverNode.setPrograde( + lastManeuverNode.getPrograde() + + ctrlManeuver.calculate(maneuverAP / targetOrbitPosition * 1000, 1000)); + maneuverAP = lastManeuverNode.getOrbit().getApoapsis(); + Thread.sleep(25); + } } - maneuverUT = ctrlManeuver.calcPID(-closestApproach, 0); - lastManeuverNode.setUT(lastManeuverNode.getUT() + maneuverUT); - System.out.println("Closest " + (closestApproach)); - closestApproach = targetVesselOrbit.distanceAtClosestApproach(lastManeuverNode.getOrbit()); + angularDiff = calculatePhaseAngle(lastManeuverNode.getOrbit().positionAt(maneuverUT, currentBodyRefFrame), + getSpaceCenter().getTargetVessel().getOrbit().positionAt(maneuverUT, currentBodyRefFrame)); + maneuverUT = lastManeuverNode.getUT(); + lastManeuverNode.setUT( + lastManeuverNode.getUT() + + ctrlManeuver.calculate(-angularDiff * 100, 0)); + System.out.println(angularDiff); Thread.sleep(25); } + // double mu = currentBody.getGravitationalParameter(); + // double time = 1000; + // + // double hohmannTransferDistance = + // lastManeuverNode.getOrbit().getSemiMajorAxis(); + // double timeOfFlight = Math.PI * Math.sqrt(Math.pow(hohmannTransferDistance, + // 3) / mu); + // double angle = activeVesselOrbit.getMeanAnomalyAtEpoch(); + // double omegaInterceptor = Math + // .sqrt(mu / + // Math.pow(activeVesselOrbit.radiusAt(getSpaceCenter().getUT()), 3)); + // // rad/s + // double omegaTarget = Math.sqrt(mu / + // Math.pow(targetVesselOrbit.radiusAt(getSpaceCenter().getUT()), 3)); // rad/s + // // double leadAngle = omegaTarget * timeOfFlight; // rad + // double leadAngle = targetVesselOrbit.getMeanAnomalyAtEpoch(); // rad + // double phaseAngle = Math.PI - leadAngle; // rad + // double calcAngle = (phaseAngle - angle); + // calcAngle = calcAngle < 0 ? calcAngle + (Math.PI * 2) : calcAngle; + // double waitTime = calcAngle / (omegaTarget - omegaInterceptor); + // time = waitTime; + // + // lastManeuverNode.setUT(getSpaceCenter().getUT() + time); + // ctrlManeuver.setOutput(-100, 100); + // ctrlManeuver.setPIDValues(0.05, 0.1, 0.01); + // double closestApproach = + // lastManeuverNode.getOrbit().distanceAtClosestApproach(targetVesselOrbit); + // System.out.println(closestApproach); + // System.out.println("Ajustando tempo de Rendezvous..."); + // while (Math.round(closestApproach) > 100) { + // if (closestApproach < 100000) { + // ctrlManeuver.setOutput(-10, 10); + // } else if (closestApproach < 10000) { + // ctrlManeuver.setOutput(-1, 1); + // } else { + // ctrlManeuver.setOutput(-100, 100); + // } + // maneuverUT = ctrlManeuver.calculate(-closestApproach, 0); + // lastManeuverNode.setUT(lastManeuverNode.getUT() + maneuverUT); + // System.out.println("Closest " + (closestApproach)); + // closestApproach = + // targetVesselOrbit.distanceAtClosestApproach(lastManeuverNode.getOrbit()); + // Thread.sleep(25); + // } // lastManeuverNode.setUT(lastManeuverNode.getUT() - // lastManeuverNode.getOrbit().getPeriod() / 2); - } catch (Exception err) { + } catch ( + + Exception err) { } } @@ -442,9 +473,9 @@ public void executeBurn(Node noDeManobra, double duracaoDaQueima) { } navigation.aimAtManeuver(noDeManobra); float limitValue = burnDvLeft > 100 ? 1000 : 100; - throttle(ctrlManeuver.calcPID((noDeManobra.getDeltaV() - Math.floor(burnDvLeft)) / + throttle(ctrlManeuver.calculate((noDeManobra.getDeltaV() - Math.floor(burnDvLeft)) / noDeManobra.getDeltaV() * limitValue, limitValue)); - Thread.sleep(50); + Thread.sleep(25); } throttle(0.0f); if (fineAdjustment) { @@ -471,7 +502,7 @@ private void adjustManeuverWithRCS(Stream> remai if (Thread.interrupted()) { throw new InterruptedException(); } - getNaveAtual().getControl().setForward((float) ctrlRCS.calcPID(-remainingDeltaV.get().getValue1() * 10, + getNaveAtual().getControl().setForward((float) ctrlRCS.calculate(-remainingDeltaV.get().getValue1() * 10, 0)); Thread.sleep(25); } @@ -496,8 +527,29 @@ private boolean canFineAdjust(String string) { return false; } + private double calculatePhaseAngle(Triplet startPos, Triplet endPos) + throws RPCException, InterruptedException { + double targetPhaseAngle = 10; + double angularDifference = 15; + Vector startPosition = new Vector(startPos); + Vector endPosition = new Vector(endPos); + + // Phase angle + double dot = endPosition.dotProduct(startPosition); + double det = endPosition.determinant(startPosition); + targetPhaseAngle = Math.atan2(det, dot); + + double targetOrbit = endPosition.magnitude(); + + double activeVesselSMA = getNaveAtual().getOrbit().getSemiMajorAxis(); + angularDifference = targetPhaseAngle + Math.PI + * (1 - (1 / (2 * Math.sqrt(2))) * Math.sqrt(Math.pow((activeVesselSMA / targetOrbit + 1), 3))); + + return Math.abs(angularDifference); + } + enum Compare { INC, AP, PE } -} \ No newline at end of file +} diff --git a/src/com/pesterenan/controllers/RoverController.java b/src/com/pesterenan/controllers/RoverController.java index f094e68..aeab195 100644 --- a/src/com/pesterenan/controllers/RoverController.java +++ b/src/com/pesterenan/controllers/RoverController.java @@ -45,8 +45,8 @@ private void initializeParameters() { roverReferenceFrame = getNaveAtual().getReferenceFrame(); roverDirection = new Vector(getNaveAtual().direction(roverReferenceFrame)); pathFinding = new PathFinding(); - acelCtrl.adjustOutput(0, 1); - sterringCtrl.adjustOutput(-1, 1); + acelCtrl.setOutput(0, 1); + sterringCtrl.setOutput(-1, 1); isAutoRoverRunning = true; } catch (RPCException ignored) { } @@ -206,10 +206,10 @@ private void driveRover() throws RPCException, IOException, StreamException { double deltaAngle = Math.abs(targetAndRadarAngle - roverAngle); getNaveAtual().getControl().setSAS(deltaAngle < 1); // Control Rover Throttle - setRoverThrottle(acelCtrl.calcPID(velHorizontal.get() / maxSpeed * 50, 50)); + setRoverThrottle(acelCtrl.calculate(velHorizontal.get() / maxSpeed * 50, 50)); // Control Rover Steering if (deltaAngle > 1) { - setRoverSteering(sterringCtrl.calcPID(roverAngle / (targetAndRadarAngle) * 100, 100)); + setRoverSteering(sterringCtrl.calculate(roverAngle / (targetAndRadarAngle) * 100, 100)); } else { setRoverSteering(0.0f); } @@ -314,4 +314,4 @@ private void setRoverSteering(double steering) throws RPCException { private enum MODE { DRIVE, NEXT_POINT, CHARGING } -} \ No newline at end of file +} diff --git a/src/com/pesterenan/utils/ControlePID.java b/src/com/pesterenan/utils/ControlePID.java index a94e5af..a5f52f8 100644 --- a/src/com/pesterenan/utils/ControlePID.java +++ b/src/com/pesterenan/utils/ControlePID.java @@ -1,47 +1,70 @@ package com.pesterenan.utils; +import krpc.client.RPCException; +import krpc.client.services.SpaceCenter; + public class ControlePID { - private double limiteMin = -1; - private double limiteMax = 1; + private SpaceCenter spaceCenter = null; + private double outputMin = -1; + private double outputMax = 1; private double kp = 0.025; private double ki = 0.001; private double kd = 0.01; - private double proportionalTerm, integralTerm, derivativeTerm = 0; - private double lastValue, lastTime = 0; + private double integralTerm = 0.0; + private double previousError, lastTime = 0.0; + private double timeSample = 0.025; + private double proportionalTerm; + private double derivativeTerm; + + public ControlePID() { + } + + public ControlePID(SpaceCenter spaceCenter, double timeSample) { + this.spaceCenter = spaceCenter; + setTimeSample(timeSample); + } + + public ControlePID(double kp, double ki, double kd, double outputMin, double outputMax) { + this.kp = kp; + this.ki = ki; + this.kd = kd; + this.outputMin = outputMin; + this.outputMax = outputMax; + } - public double calcPID(double currentValue, double limitValue) { - double now = System.currentTimeMillis(); - double changeInTime = now - this.lastTime; - double timeSample = 25; + public double calculate(double currentValue, double setPoint) { + double now = this.getCurrentTime(); + double changeInTime = now - lastTime; + System.out.println(changeInTime); if (changeInTime >= timeSample) { - double error = limitValue - currentValue; - double changeInValues = (currentValue - this.lastValue); - - this.proportionalTerm = this.kp * error; - this.integralTerm = limitOutput(this.integralTerm + ki * error); - this.derivativeTerm = kd * -changeInValues; - this.lastValue = currentValue; - this.lastTime = now; + double error = setPoint - currentValue; + proportionalTerm = kp * error; + + integralTerm += ki * error; + integralTerm = limitOutput(integralTerm); + + derivativeTerm = kd * (error - previousError); + previousError = error; + lastTime = now; } - return limitOutput(proportionalTerm + ki * this.integralTerm + derivativeTerm); + return limitOutput(proportionalTerm + integralTerm + derivativeTerm); } - private double limitOutput(double valor) { - return Utilities.clamp(valor, this.limiteMin, this.limiteMax); + private double limitOutput(double value) { + return Utilities.clamp(value, outputMin, outputMax); } - public void adjustOutput(double min, double max) { + public void setOutput(double min, double max) { if (min > max) { return; } - this.limiteMin = min; - this.limiteMax = max; - this.integralTerm = limitOutput(this.integralTerm); - + outputMin = min; + outputMax = max; + integralTerm = limitOutput(integralTerm); } - public void adjustPID(double Kp, double Ki, double Kd) { + public void setPIDValues(double Kp, double Ki, double Kd) { if (Kp > 0) { this.kp = Kp; } @@ -53,4 +76,17 @@ public void adjustPID(double Kp, double Ki, double Kd) { } } -} \ No newline at end of file + public void setTimeSample(double milliseconds) { + timeSample = milliseconds > 0 ? milliseconds / 1000 : timeSample; + } + + private double getCurrentTime() { + try { + return spaceCenter.getUT(); + } catch (RPCException | NullPointerException ignored) { + System.err.println("Não foi possível buscar o tempo do jogo, retornando do sistema"); + return System.currentTimeMillis(); + } + } + +} diff --git a/src/com/pesterenan/utils/Vector.java b/src/com/pesterenan/utils/Vector.java index c47661d..06c69e4 100644 --- a/src/com/pesterenan/utils/Vector.java +++ b/src/com/pesterenan/utils/Vector.java @@ -66,7 +66,7 @@ public static double distance(Vector start, Vector end) { * @param start - Vetor contendo os componentes da posição do ponto de origem. * @param end - Vetor contendo os componentes da posição do alvo. * @return - Vetor com a soma dos valores do ponto de origem com os valores do - * alvo. + * alvo. */ public static Vector targetDirection(Vector start, Vector end) { return end.subtract(start).normalize(); @@ -78,7 +78,7 @@ public static Vector targetDirection(Vector start, Vector end) { * @param start - Tupla contendo os componentes da posição do ponto de origem. * @param end - Tupla contendo os componentes da posição do alvo. * @return - Vetor inverso, com a soma dos valores do ponto de origem com o - * negativo dos valores do alvo. + * negativo dos valores do alvo. */ public static Vector targetOppositeDirection(Vector start, Vector end) { return end.subtract(start).multiply(-1).normalize(); @@ -151,12 +151,13 @@ public Vector normalize() { * @param otherVector - Vetor para somar os componentes * @return Novo vetor com a soma dos componentes dos dois */ - public double dotP(Vector otherVector) { + + public double dotProduct(Vector otherVector) { return (x * otherVector.x + y * otherVector.y + z * otherVector.z); } public double determinant(Vector otherVector) { - return (x * otherVector.y - y * otherVector.x - z * otherVector.z); + return (x * otherVector.z - y * otherVector.y - z * otherVector.x); } /** @@ -184,8 +185,8 @@ public Vector subtract(Vector otherVector) { * * @param scalar - Fator para multiplicar os componentes * @return Novo vetor com os componentes multiplicados pela escalar. Caso a - * escalar informada for 0, o Vetor retornado terá 0 como seus - * componentes. + * escalar informada for 0, o Vetor retornado terá 0 como seus + * componentes. */ public Vector multiply(double scalar) { if (scalar != 0) { @@ -199,7 +200,7 @@ public Vector multiply(double scalar) { * * @param scalar - Fator para dividir os componentes * @return Novo vetor com os componentes divididos pela escalar. Caso a escalar - * informada for 0, o Vetor retornado terá 0 como seus componentes. + * informada for 0, o Vetor retornado terá 0 como seus componentes. */ public Vector divide(double scalar) { if (scalar != 0) { diff --git a/src/com/pesterenan/views/CreateManeuverJPanel.java b/src/com/pesterenan/views/CreateManeuverJPanel.java index db253de..6c3a5ae 100644 --- a/src/com/pesterenan/views/CreateManeuverJPanel.java +++ b/src/com/pesterenan/views/CreateManeuverJPanel.java @@ -80,7 +80,7 @@ public void initComponents() { sliderValues.put(4, 100f); sliderValues.put(5, 1000f); - ctrlManeuver.adjustOutput(-100, 100); + ctrlManeuver.setOutput(-100, 100); } @Override From 8133220aaaa3851065e49bcd1fc07f64af9bb391 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Fri, 13 Oct 2023 21:19:51 -0300 Subject: [PATCH 13/19] [Docking]: Finally have a working docking script? --- src/com/pesterenan/Main.java | 176 ++++++++++++------ .../controllers/LandingController.java | 37 ++-- src/com/pesterenan/utils/ControlePID.java | 18 +- 3 files changed, 149 insertions(+), 82 deletions(-) diff --git a/src/com/pesterenan/Main.java b/src/com/pesterenan/Main.java index 76884c4..eb1265d 100644 --- a/src/com/pesterenan/Main.java +++ b/src/com/pesterenan/Main.java @@ -2,8 +2,6 @@ import java.io.IOException; -import org.javatuples.Triplet; - import com.pesterenan.utils.ControlePID; import com.pesterenan.utils.Vector; @@ -35,12 +33,16 @@ public class Main { private static ReferenceFrame orbitalRefVessel; private static ReferenceFrame vesselRefFrame; private static ReferenceFrame orbitalRefBody; - private static final double SPEED_LIMIT = 5.0; + private static final double SPEED_LIMIT = 0.5; private static final double DISTANCE_LIMIT = 50.0; private static Line distanceLine; private static Line distLineXAxis; private static Line distLineYAxis; private static Line distLineZAxis; + private static DockingPort myDockingPort; + private static DockingPort targetDockingPort; + private static Vector positionMyDockingPort; + private static Vector positionTargetDockingPort; private static void initializeParameters() { try { @@ -54,12 +56,23 @@ private static void initializeParameters() { orbitalRefBody = activeVessel.getOrbit().getBody().getReferenceFrame(); speedCtrl = new ControlePID(spaceCenter, 50); - speedCtrl.setPIDValues(0.5, 0.001, 0.01); distCtrl = new ControlePID(spaceCenter, 50); - distCtrl.setPIDValues(1, 0.001, 0.1); RCSForwardsCtrl = new ControlePID(spaceCenter, 50); RCSSidewaysCtrl = new ControlePID(spaceCenter, 50); RCSUpwardsCtrl = new ControlePID(spaceCenter, 50); + speedCtrl.setPIDValues(0.025, 0.001, 0.01); + distCtrl.setPIDValues(1, 0.001, 0.1); + RCSForwardsCtrl.setPIDValues(1, 0.001, 0.01); + RCSSidewaysCtrl.setPIDValues(0.025, 0.001, 10); + RCSUpwardsCtrl.setPIDValues(1, 0.001, 0.01); + + myDockingPort = activeVessel.getParts().getDockingPorts().get(0); + targetDockingPort = targetVessel.getParts().getDockingPorts().get(0); + + positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); + positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); + + createLines(positionMyDockingPort, positionTargetDockingPort); } catch (IOException | RPCException e) { e.printStackTrace(); @@ -71,61 +84,52 @@ public static void main(String[] args) { // Initialize parameters for the script, connection and setup: initializeParameters(); - DockingPort myDockingPort = activeVessel.getParts().getDockingPorts().get(0); - DockingPort targetDockingPort = targetVessel.getParts().getDockingPorts().get(0); - - // Fase 1: Apontar pra dockingport alvo de longe: - Vector positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); - Vector positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); - - createLines(positionMyDockingPort, positionTargetDockingPort); - // Setting up the control control = activeVessel.getControl(); control.setSAS(true); control.setSASMode(SASMode.STABILITY_ASSIST); double loopTimeInSeconds = 0; - while (loopTimeInSeconds < 30) { - // Calcular distancia: - double distanceBetweenPortsInMeters = positionTargetDockingPort.subtract(positionMyDockingPort) - .magnitude(); - double currentRelativeVelocity = new Vector(activeVessel.velocity( - targetDockingPort.getReferenceFrame())).magnitude(); - positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); - positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); - - double speedOutput = 0; - double distanceOutput = 0; - - setDirection(distanceBetweenPortsInMeters); - if (distanceBetweenPortsInMeters > DISTANCE_LIMIT) { - speedOutput = speedCtrl.calculate(currentRelativeVelocity, SPEED_LIMIT); - distanceOutput = distCtrl.calculate(-distanceBetweenPortsInMeters, - DISTANCE_LIMIT); - System.out.println("Speed: " + speedOutput + " Distance: " + distanceOutput + - " Calc: " - + (speedOutput * distanceOutput)); - control.setForward( - (float) (speedOutput * distanceOutput)); - } else { - if (currentRelativeVelocity <= 3.0) { - speedOutput = speedCtrl.calculate(currentRelativeVelocity, 0.1); - control.setForward((float) (speedOutput)); - if (currentRelativeVelocity < 0.3) { - break; - } - } else { - speedOutput = speedCtrl.calculate(currentRelativeVelocity, 2); - control.setForward((float) (speedOutput * 0.5)); - } - System.out.println("Vel: " + currentRelativeVelocity + " PID: " + - speedOutput); - } - updateLines(positionMyDockingPort, positionTargetDockingPort); - Thread.sleep(50); - loopTimeInSeconds += 0.05; - } + // while (loopTimeInSeconds < 30) { + // // Calcular distancia: + // double distanceBetweenPortsInMeters = + // positionTargetDockingPort.subtract(positionMyDockingPort) + // .magnitude(); + // double currentRelativeVelocity = new Vector(activeVessel.velocity( + // targetDockingPort.getReferenceFrame())).magnitude(); + // positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); + // positionTargetDockingPort = new + // Vector(targetDockingPort.position(vesselRefFrame)); + // + // double speedOutput = 0; + // double distanceOutput = 0; + // + // setDirection(distanceBetweenPortsInMeters); + // if (distanceBetweenPortsInMeters > DISTANCE_LIMIT) { + // speedOutput = RCSForwardsCtrl.calculate(currentRelativeVelocity, + // SPEED_LIMIT); + // distanceOutput = distCtrl.calculate(-distanceBetweenPortsInMeters, + // DISTANCE_LIMIT); + // control.setForward( + // (float) (speedOutput * distanceOutput)); + // } else { + // if (currentRelativeVelocity <= 3.0) { + // speedOutput = RCSForwardsCtrl.calculate(currentRelativeVelocity, 0); + // control.setForward((float) (speedOutput)); + // if (currentRelativeVelocity < 0.3) { + // break; + // } + // } else { + // speedOutput = RCSForwardsCtrl.calculate(currentRelativeVelocity, 2); + // control.setForward((float) (speedOutput)); + // } + // System.out.println("Vel: " + currentRelativeVelocity + " PID: " + + // speedOutput); + // } + // updateLines(positionMyDockingPort, positionTargetDockingPort); + // Thread.sleep(50); + // loopTimeInSeconds += 0.05; + // } // Triplet direcaoTransformada = spaceCenter // .transformDirection(targetDockingPort.direction(orbitalRefVessel), @@ -140,6 +144,7 @@ public static void main(String[] args) { activeVessel.getAutoPilot().engage(); activeVessel.getAutoPilot().setReferenceFrame(orbitalRefVessel); activeVessel.getAutoPilot().setTargetDirection(direcaoContrariaDockingPortAlvo.toTriplet()); + activeVessel.getAutoPilot().setTargetRoll(90); loopTimeInSeconds = 0; while (loopTimeInSeconds < 10) { @@ -148,21 +153,78 @@ public static void main(String[] args) { if (activeVessel.getAutoPilot().getError() < 1) { break; } + updateLines(positionMyDockingPort, positionTargetDockingPort); Thread.sleep(50); loopTimeInSeconds += 0.05; } + activeVessel.getAutoPilot().disengage(); control.setSAS(true); control.setSASMode(SASMode.STABILITY_ASSIST); - activeVessel.getAutoPilot().disengage(); Thread.sleep(1000); + control.setRCS(true); + loopTimeInSeconds = 0; + positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); + positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); + Vector distanceBetweenPorts = positionMyDockingPort.subtract(positionTargetDockingPort); + + double lastXTargetPos = distanceBetweenPorts.x; + double lastYTargetPos = distanceBetweenPorts.y; + double lastZTargetPos = distanceBetweenPorts.z; + while (true) { + positionMyDockingPort = new Vector(activeVessel.position(vesselRefFrame)); + positionTargetDockingPort = new Vector(targetVessel.position(vesselRefFrame)); + distanceBetweenPorts = positionMyDockingPort.subtract(positionTargetDockingPort); + + adjustPID(); + + double currentXAxisSpeed = (distanceBetweenPorts.x - lastXTargetPos) * 10; + double currentYAxisSpeed = Math.abs((distanceBetweenPorts.y - lastYTargetPos)); + double currentZAxisSpeed = Math.abs((distanceBetweenPorts.z - lastZTargetPos)); + System.out.println(String.format("%.2f", currentXAxisSpeed) + " X " + distanceBetweenPorts.x + " X"); + + float sidewaysError = (float) RCSSidewaysCtrl.calculate(distanceBetweenPorts.x * 10, 0); + float upwardsError = (float) RCSUpwardsCtrl.calculate(distanceBetweenPorts.z * 10, 0); + float forwardsError = (float) RCSForwardsCtrl.calculate(distanceBetweenPorts.y * 20, 0); + + boolean shouldGetCloser = Math.abs(distanceBetweenPorts.x) < 0.3 + && Math.abs(distanceBetweenPorts.z) < 0.3; + + if (shouldGetCloser) { + forwardsError = (float) RCSForwardsCtrl.calculate(distanceBetweenPorts.y * 20, 0); + control.setForward((float) forwardsError); + } else { + forwardsError = (float) RCSForwardsCtrl.calculate(currentZAxisSpeed, 0); + control.setForward((float) forwardsError); + control.setRight((float) sidewaysError); + control.setUp((float) -upwardsError); + } + + // Guardando últimas posições: + lastXTargetPos = distanceBetweenPorts.x; + lastYTargetPos = distanceBetweenPorts.y; + lastZTargetPos = distanceBetweenPorts.z; + + updateLines(positionMyDockingPort, positionTargetDockingPort); + Thread.sleep(50); + loopTimeInSeconds += 0.05; + + } // // Close the connection when finished - connection.close(); - } catch (IOException | RPCException | InterruptedException e) { + // connection.close(); + } catch (RPCException | InterruptedException e) { e.printStackTrace(); } } + // distancia x5 , p: 0.05, i: 0.0001, d: p x2 + private static void adjustPID() { + RCSUpwardsCtrl.setPIDValues(0.1, 0.0002, 0.2); + RCSSidewaysCtrl.setPIDValues(0.1, 0.0002, 0.2); + + RCSForwardsCtrl.setPIDValues(0.2, 0.0004, 0.4); + } + private static void setDirection(double distance) { try { if (distance > DISTANCE_LIMIT) { diff --git a/src/com/pesterenan/controllers/LandingController.java b/src/com/pesterenan/controllers/LandingController.java index 81761bb..249e73b 100644 --- a/src/com/pesterenan/controllers/LandingController.java +++ b/src/com/pesterenan/controllers/LandingController.java @@ -17,13 +17,13 @@ public class LandingController extends Controller { public static final double MAX_VELOCITY = 5; - private static final double velP = 0.025; - private static final double velI = 0.001; - private static final double velD = 0.01; + private static final double velP = 0.05; + private static final double velI = 0.005; + private static final double velD = 0.001; private ControlePID altitudeCtrl; private ControlePID velocityCtrl; private Navigation navigation; - private final int HUNDRED_PERCENT = 200; + private final int HUNDRED_PERCENT = 100; private double hoverAltitude; private boolean hoveringMode; private boolean hoverAfterApproximation; @@ -40,8 +40,8 @@ public LandingController(Map commands) { } private void initializeParameters() { - altitudeCtrl = new ControlePID(getSpaceCenter(), 25); - velocityCtrl = new ControlePID(getSpaceCenter(), 25); + altitudeCtrl = new ControlePID(getSpaceCenter(), 50); + velocityCtrl = new ControlePID(getSpaceCenter(), 50); altitudeCtrl.setOutput(0, 1); velocityCtrl.setOutput(0, 1); } @@ -75,12 +75,11 @@ private void hoverArea() { currentMode = MODE.GOING_UP; } else { currentMode = MODE.HOVERING; - currentMode = MODE.HOVERING; } changeControlMode(); } catch (RPCException | StreamException ignored) { } - Thread.sleep(25); + Thread.sleep(50); } } catch (InterruptedException | RPCException ignored) { // disengageAfterException(Bundle.getString("status_liftoff_abort")); @@ -108,7 +107,7 @@ private void autoLanding() { } getNaveAtual().getControl().setBrakes(true); changeControlMode(); - Thread.sleep(25); + Thread.sleep(50); } } catch (RPCException | StreamException | InterruptedException e) { setCurrentStatus(Bundle.getString("status_ready")); @@ -117,7 +116,7 @@ private void autoLanding() { private void changeControlMode() throws RPCException, StreamException, InterruptedException { adjustPIDbyTWR(); - double velPID, altPID; + double velPID, altPID = 0; // Change vessel behavior depending on which mode is active switch (currentMode) { case DEORBITING: @@ -141,11 +140,11 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt double threshold = Utilities.clamp( ((currentVelocity + zeroVelocity) - landingDistanceThreshold) / landingDistanceThreshold, 0, 1); - altPID = altitudeCtrl.calculate(currentVelocity / zeroVelocity * HUNDRED_PERCENT, HUNDRED_PERCENT); + altPID = altitudeCtrl.calculate(currentVelocity - zeroVelocity, 0); velPID = velocityCtrl.calculate(velVertical.get(), -Utilities.clamp(altitudeSup.get() * 0.1, 1, 10)); throttle(Utilities.linearInterpolation(velPID, altPID, threshold)); navigation.aimForLanding(); - if (threshold < 0.25 || altitudeSup.get() < landingDistanceThreshold) { + if (threshold < 0.10 || altitudeSup.get() < landingDistanceThreshold) { hoverAltitude = landingDistanceThreshold; getNaveAtual().getControl().setGear(true); if (hoverAfterApproximation) { @@ -155,6 +154,7 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt } currentMode = MODE.LANDING; } + System.out.println("altCtrl: " + altPID); setCurrentStatus("Se aproximando do momento do pouso..."); break; case GOING_UP: @@ -173,7 +173,7 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt break; case LANDING: controlThrottleByMatchingVerticalVelocity( - velHorizontal.get() > 2 ? 0 : -Utilities.clamp(altitudeSup.get() * 0.1, 1, 10)); + velHorizontal.get() > 4 ? 0 : -Utilities.clamp(altitudeSup.get() * 0.1, 1, 10)); navigation.aimForLanding(); setCurrentStatus("Pousando..."); hasTheVesselLanded(); @@ -193,7 +193,7 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt private void controlThrottleByMatchingVerticalVelocity(double velocityToMatch) throws RPCException, StreamException { velocityCtrl.setOutput(0, 1); - throttle(velocityCtrl.calculate(velVertical.get(), velocityToMatch)); + throttle(velocityCtrl.calculate(velVertical.get(), velocityToMatch + velHorizontal.get())); } private void deOrbitShip() throws RPCException, StreamException, InterruptedException { @@ -207,7 +207,7 @@ private void deOrbitShip() throws RPCException, StreamException, InterruptedExce navigation.aimForLanding(); setCurrentStatus(Bundle.getString("status_orienting_ship")); ap.wait_(); - Thread.sleep(25); + Thread.sleep(50); } double targetPeriapsis = currentBody.getAtmosphereDepth(); targetPeriapsis = targetPeriapsis > 0 ? targetPeriapsis / 2 : -currentBody.getEquatorialRadius() / 2; @@ -215,7 +215,7 @@ private void deOrbitShip() throws RPCException, StreamException, InterruptedExce navigation.aimForLanding(); throttle(altitudeCtrl.calculate(targetPeriapsis, periastro.get())); setCurrentStatus(Bundle.getString("status_lowering_periapsis")); - Thread.sleep(25); + Thread.sleep(50); } getNaveAtual().getControl().setRCS(false); throttle(0.0f); @@ -227,8 +227,9 @@ private void deOrbitShip() throws RPCException, StreamException, InterruptedExce */ private void adjustPIDbyTWR() throws RPCException, StreamException { double currentTWR = Math.min(getTWR(), maxTWR); - velocityCtrl.setPIDValues(currentTWR * velP, velI, velD); - altitudeCtrl.setPIDValues(currentTWR * velP, velI, velD); + // double currentTWR = getMaxAcel(maxTWR); + altitudeCtrl.setPIDValues(currentTWR * 0.05, 10, 0.001); + velocityCtrl.setPIDValues(currentTWR * 0.005, 0.1, 0.001); } private double calculateCurrentVelocityMagnitude() throws RPCException, StreamException { diff --git a/src/com/pesterenan/utils/ControlePID.java b/src/com/pesterenan/utils/ControlePID.java index a5f52f8..337f414 100644 --- a/src/com/pesterenan/utils/ControlePID.java +++ b/src/com/pesterenan/utils/ControlePID.java @@ -11,8 +11,8 @@ public class ControlePID { private double ki = 0.001; private double kd = 0.01; private double integralTerm = 0.0; - private double previousError, lastTime = 0.0; - private double timeSample = 0.025; + private double previousError, previousMeasurement, lastTime = 0.0; + private double timeSample = 0.025; // 25 millisegundos private double proportionalTerm; private double derivativeTerm; @@ -32,19 +32,23 @@ public ControlePID(double kp, double ki, double kd, double outputMin, double out this.outputMax = outputMax; } - public double calculate(double currentValue, double setPoint) { + public double calculate(double measurement, double setPoint) { double now = this.getCurrentTime(); double changeInTime = now - lastTime; - System.out.println(changeInTime); if (changeInTime >= timeSample) { - double error = setPoint - currentValue; + // Error signal + double error = setPoint - measurement; + // Proportional proportionalTerm = kp * error; - integralTerm += ki * error; + // Integral + // integralTerm += 0.5f * ki * timeSample * (error + previousError); + // integralTerm += ki * (error + previousError); + integralTerm = ki * (integralTerm + (error * timeSample)); integralTerm = limitOutput(integralTerm); - derivativeTerm = kd * (error - previousError); + derivativeTerm = kd * ((error - previousError) / timeSample); previousError = error; lastTime = now; } From 63380598ccc6961bf56f45964e135bf4666d3ff9 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Thu, 19 Oct 2023 21:00:59 -0300 Subject: [PATCH 14/19] [Docking]: Working 1st and 2nd docking stage --- src/com/pesterenan/Main.java | 176 +++++++++++++++++------------------ 1 file changed, 86 insertions(+), 90 deletions(-) diff --git a/src/com/pesterenan/Main.java b/src/com/pesterenan/Main.java index eb1265d..e3dd565 100644 --- a/src/com/pesterenan/Main.java +++ b/src/com/pesterenan/Main.java @@ -3,6 +3,7 @@ import java.io.IOException; import com.pesterenan.utils.ControlePID; +import com.pesterenan.utils.Utilities; import com.pesterenan.utils.Vector; import krpc.client.Connection; @@ -33,7 +34,7 @@ public class Main { private static ReferenceFrame orbitalRefVessel; private static ReferenceFrame vesselRefFrame; private static ReferenceFrame orbitalRefBody; - private static final double SPEED_LIMIT = 0.5; + private static final double SPEED_LIMIT = 5.0; private static final double DISTANCE_LIMIT = 50.0; private static Line distanceLine; private static Line distLineXAxis; @@ -60,17 +61,14 @@ private static void initializeParameters() { RCSForwardsCtrl = new ControlePID(spaceCenter, 50); RCSSidewaysCtrl = new ControlePID(spaceCenter, 50); RCSUpwardsCtrl = new ControlePID(spaceCenter, 50); - speedCtrl.setPIDValues(0.025, 0.001, 0.01); distCtrl.setPIDValues(1, 0.001, 0.1); - RCSForwardsCtrl.setPIDValues(1, 0.001, 0.01); - RCSSidewaysCtrl.setPIDValues(0.025, 0.001, 10); - RCSUpwardsCtrl.setPIDValues(1, 0.001, 0.01); + adjustPID(25); myDockingPort = activeVessel.getParts().getDockingPorts().get(0); targetDockingPort = targetVessel.getParts().getDockingPorts().get(0); - positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); - positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); + positionMyDockingPort = new Vector(myDockingPort.position(orbitalRefVessel)); + positionTargetDockingPort = new Vector(targetDockingPort.position(orbitalRefVessel)); createLines(positionMyDockingPort, positionTargetDockingPort); @@ -87,54 +85,53 @@ public static void main(String[] args) { // Setting up the control control = activeVessel.getControl(); control.setSAS(true); + control.setRCS(false); control.setSASMode(SASMode.STABILITY_ASSIST); double loopTimeInSeconds = 0; - // while (loopTimeInSeconds < 30) { - // // Calcular distancia: - // double distanceBetweenPortsInMeters = - // positionTargetDockingPort.subtract(positionMyDockingPort) - // .magnitude(); - // double currentRelativeVelocity = new Vector(activeVessel.velocity( - // targetDockingPort.getReferenceFrame())).magnitude(); - // positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); - // positionTargetDockingPort = new - // Vector(targetDockingPort.position(vesselRefFrame)); - // - // double speedOutput = 0; - // double distanceOutput = 0; - // - // setDirection(distanceBetweenPortsInMeters); - // if (distanceBetweenPortsInMeters > DISTANCE_LIMIT) { - // speedOutput = RCSForwardsCtrl.calculate(currentRelativeVelocity, - // SPEED_LIMIT); - // distanceOutput = distCtrl.calculate(-distanceBetweenPortsInMeters, - // DISTANCE_LIMIT); - // control.setForward( - // (float) (speedOutput * distanceOutput)); - // } else { - // if (currentRelativeVelocity <= 3.0) { - // speedOutput = RCSForwardsCtrl.calculate(currentRelativeVelocity, 0); - // control.setForward((float) (speedOutput)); - // if (currentRelativeVelocity < 0.3) { - // break; - // } - // } else { - // speedOutput = RCSForwardsCtrl.calculate(currentRelativeVelocity, 2); - // control.setForward((float) (speedOutput)); - // } - // System.out.println("Vel: " + currentRelativeVelocity + " PID: " + - // speedOutput); - // } - // updateLines(positionMyDockingPort, positionTargetDockingPort); - // Thread.sleep(50); - // loopTimeInSeconds += 0.05; - // } - - // Triplet direcaoTransformada = spaceCenter - // .transformDirection(targetDockingPort.direction(orbitalRefVessel), - // orbitalRefVessel, - // orbitalRefVessel); + Vector targetDirection = positionMyDockingPort.subtract(positionTargetDockingPort).multiply(-1); + activeVessel.getAutoPilot().engage(); + activeVessel.getAutoPilot().setReferenceFrame(orbitalRefVessel); + activeVessel.getAutoPilot().setTargetDirection(targetDirection.toTriplet()); + activeVessel.getAutoPilot().setTargetRoll(90); + activeVessel.getAutoPilot().wait_(); + control.setRCS(true); + // PRIMEIRA PARTE DO DOCKING: APROXIMAÇÃO + positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); + positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); + + targetDirection = positionMyDockingPort.subtract(positionTargetDockingPort); + + double lastYTargetPos = targetDirection.y; + System.out.println(lastYTargetPos + " LAST Y POS"); + long sleepTime = 25; + while (Math.abs(lastYTargetPos) >= DISTANCE_LIMIT) { + // Calcular distancia: + double distanceBetweenPortsInMeters = positionTargetDockingPort.subtract(positionMyDockingPort) + .magnitude(); + double currentRelativeVelocity = new Vector(activeVessel.velocity( + targetDockingPort.getReferenceFrame())).magnitude(); + // Buscar posições atuais: + positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); + positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); + + targetDirection = positionMyDockingPort.subtract(positionTargetDockingPort); + + // Calcular velocidade de cada eixo: + double currentYAxisSpeed = (targetDirection.y - lastYTargetPos) * sleepTime; + + // Calcular o valor PID de cada eixo: + float forwardsError = (float) RCSForwardsCtrl.calculate(targetDirection.y, + Math.signum(targetDirection.y) * DISTANCE_LIMIT); + float speedError = (float) speedCtrl.calculate(currentYAxisSpeed / 10, 0); + + control.setForward(forwardsError); + + updateLines(positionMyDockingPort, positionTargetDockingPort); + Thread.sleep(sleepTime); + loopTimeInSeconds += 0.05; + lastYTargetPos = targetDirection.y; + } // SEGUNDA PARTE APONTAR PRO LADO CONTRARIO: Vector direcaoContrariaDockingPortAlvo = new Vector(targetDockingPort.direction(orbitalRefVessel)) @@ -145,59 +142,50 @@ public static void main(String[] args) { activeVessel.getAutoPilot().setReferenceFrame(orbitalRefVessel); activeVessel.getAutoPilot().setTargetDirection(direcaoContrariaDockingPortAlvo.toTriplet()); activeVessel.getAutoPilot().setTargetRoll(90); - loopTimeInSeconds = 0; - - while (loopTimeInSeconds < 10) { - System.out.println("apontando"); - activeVessel.getAutoPilot().wait_(); - if (activeVessel.getAutoPilot().getError() < 1) { - break; - } - updateLines(positionMyDockingPort, positionTargetDockingPort); - Thread.sleep(50); - loopTimeInSeconds += 0.05; - } + activeVessel.getAutoPilot().wait_(); activeVessel.getAutoPilot().disengage(); control.setSAS(true); control.setSASMode(SASMode.STABILITY_ASSIST); Thread.sleep(1000); control.setRCS(true); - loopTimeInSeconds = 0; positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); Vector distanceBetweenPorts = positionMyDockingPort.subtract(positionTargetDockingPort); + loopTimeInSeconds = 0; double lastXTargetPos = distanceBetweenPorts.x; - double lastYTargetPos = distanceBetweenPorts.y; + lastYTargetPos = distanceBetweenPorts.y; double lastZTargetPos = distanceBetweenPorts.z; + sleepTime = 50; while (true) { - positionMyDockingPort = new Vector(activeVessel.position(vesselRefFrame)); - positionTargetDockingPort = new Vector(targetVessel.position(vesselRefFrame)); + // Buscar posições atuais: + positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); + positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); distanceBetweenPorts = positionMyDockingPort.subtract(positionTargetDockingPort); - adjustPID(); + // Calcular velocidade de cada eixo: + double currentXAxisSpeed = (distanceBetweenPorts.x - lastXTargetPos) * sleepTime; + double currentYAxisSpeed = (distanceBetweenPorts.y - lastYTargetPos) * sleepTime; + double currentZAxisSpeed = (distanceBetweenPorts.z - lastZTargetPos) * sleepTime; - double currentXAxisSpeed = (distanceBetweenPorts.x - lastXTargetPos) * 10; - double currentYAxisSpeed = Math.abs((distanceBetweenPorts.y - lastYTargetPos)); - double currentZAxisSpeed = Math.abs((distanceBetweenPorts.z - lastZTargetPos)); - System.out.println(String.format("%.2f", currentXAxisSpeed) + " X " + distanceBetweenPorts.x + " X"); - - float sidewaysError = (float) RCSSidewaysCtrl.calculate(distanceBetweenPorts.x * 10, 0); - float upwardsError = (float) RCSUpwardsCtrl.calculate(distanceBetweenPorts.z * 10, 0); - float forwardsError = (float) RCSForwardsCtrl.calculate(distanceBetweenPorts.y * 20, 0); + // Calcular o valor PID de cada eixo: + float sidewaysError = (float) RCSSidewaysCtrl.calculate(distanceBetweenPorts.x, 0); + float upwardsError = (float) RCSUpwardsCtrl.calculate(distanceBetweenPorts.z, 0); + float forwardsError = (float) RCSForwardsCtrl.calculate(distanceBetweenPorts.y, 0); + float speedError = (float) speedCtrl.calculate(currentYAxisSpeed / 10, 0); boolean shouldGetCloser = Math.abs(distanceBetweenPorts.x) < 0.3 && Math.abs(distanceBetweenPorts.z) < 0.3; if (shouldGetCloser) { - forwardsError = (float) RCSForwardsCtrl.calculate(distanceBetweenPorts.y * 20, 0); - control.setForward((float) forwardsError); + control.setForward(forwardsError); + control.setRight(0); + control.setUp(0); } else { - forwardsError = (float) RCSForwardsCtrl.calculate(currentZAxisSpeed, 0); - control.setForward((float) forwardsError); - control.setRight((float) sidewaysError); - control.setUp((float) -upwardsError); + control.setForward(speedError); + control.setRight(sidewaysError); + control.setUp(-upwardsError); } // Guardando últimas posições: @@ -205,10 +193,10 @@ public static void main(String[] args) { lastYTargetPos = distanceBetweenPorts.y; lastZTargetPos = distanceBetweenPorts.z; + adjustPID(sleepTime); updateLines(positionMyDockingPort, positionTargetDockingPort); - Thread.sleep(50); loopTimeInSeconds += 0.05; - + Thread.sleep(sleepTime); } // // Close the connection when finished // connection.close(); @@ -218,11 +206,19 @@ public static void main(String[] args) { } // distancia x5 , p: 0.05, i: 0.0001, d: p x2 - private static void adjustPID() { - RCSUpwardsCtrl.setPIDValues(0.1, 0.0002, 0.2); - RCSSidewaysCtrl.setPIDValues(0.1, 0.0002, 0.2); - - RCSForwardsCtrl.setPIDValues(0.2, 0.0004, 0.4); + private static void adjustPID(double timeSample) { + double ms = 1000 / timeSample; + double pGain = 4 / ms; + double iGain = pGain / 500; + double dGain = pGain * 2; + speedCtrl.setTimeSample(timeSample); + speedCtrl.setPIDValues(pGain, iGain, dGain); + RCSUpwardsCtrl.setTimeSample(timeSample); + RCSUpwardsCtrl.setPIDValues(pGain, iGain, dGain); + RCSSidewaysCtrl.setTimeSample(timeSample); + RCSSidewaysCtrl.setPIDValues(pGain, iGain, dGain); + RCSForwardsCtrl.setTimeSample(timeSample); + RCSForwardsCtrl.setPIDValues(pGain, iGain, dGain); } private static void setDirection(double distance) { From c8902d813d33c4b85f847da2c6b0a68c690aea07 Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Thu, 9 Nov 2023 22:25:57 -0300 Subject: [PATCH 15/19] [Docking]: We have a docking script! --- src/com/pesterenan/Main.java | 188 +++++++----------- .../controllers/LandingController.java | 44 ++-- src/com/pesterenan/utils/ControlePID.java | 8 + 3 files changed, 108 insertions(+), 132 deletions(-) diff --git a/src/com/pesterenan/Main.java b/src/com/pesterenan/Main.java index e3dd565..ceb84df 100644 --- a/src/com/pesterenan/Main.java +++ b/src/com/pesterenan/Main.java @@ -2,7 +2,6 @@ import java.io.IOException; -import com.pesterenan.utils.ControlePID; import com.pesterenan.utils.Utilities; import com.pesterenan.utils.Vector; @@ -21,11 +20,6 @@ public class Main { private static Connection connection; private static SpaceCenter spaceCenter; private static Drawing drawing; - private static ControlePID speedCtrl; - private static ControlePID distCtrl; - private static ControlePID RCSForwardsCtrl; - private static ControlePID RCSSidewaysCtrl; - private static ControlePID RCSUpwardsCtrl; private static Control control; private static Vessel activeVessel; @@ -34,8 +28,8 @@ public class Main { private static ReferenceFrame orbitalRefVessel; private static ReferenceFrame vesselRefFrame; private static ReferenceFrame orbitalRefBody; - private static final double SPEED_LIMIT = 5.0; - private static final double DISTANCE_LIMIT = 50.0; + private static final double SPEED_LIMIT = 3.0; + private static final double DISTANCE_LIMIT = 25.0; private static Line distanceLine; private static Line distLineXAxis; private static Line distLineYAxis; @@ -56,14 +50,6 @@ private static void initializeParameters() { orbitalRefVessel = activeVessel.getOrbitalReferenceFrame(); orbitalRefBody = activeVessel.getOrbit().getBody().getReferenceFrame(); - speedCtrl = new ControlePID(spaceCenter, 50); - distCtrl = new ControlePID(spaceCenter, 50); - RCSForwardsCtrl = new ControlePID(spaceCenter, 50); - RCSSidewaysCtrl = new ControlePID(spaceCenter, 50); - RCSUpwardsCtrl = new ControlePID(spaceCenter, 50); - distCtrl.setPIDValues(1, 0.001, 0.1); - adjustPID(25); - myDockingPort = activeVessel.getParts().getDockingPorts().get(0); targetDockingPort = targetVessel.getParts().getDockingPorts().get(0); @@ -88,49 +74,58 @@ public static void main(String[] args) { control.setRCS(false); control.setSASMode(SASMode.STABILITY_ASSIST); - double loopTimeInSeconds = 0; - Vector targetDirection = positionMyDockingPort.subtract(positionTargetDockingPort).multiply(-1); - activeVessel.getAutoPilot().engage(); + Vector targetDirection = new Vector(activeVessel.position(orbitalRefVessel)) + .subtract(new Vector(targetVessel.position(orbitalRefVessel))).multiply(-1); activeVessel.getAutoPilot().setReferenceFrame(orbitalRefVessel); activeVessel.getAutoPilot().setTargetDirection(targetDirection.toTriplet()); activeVessel.getAutoPilot().setTargetRoll(90); - activeVessel.getAutoPilot().wait_(); + activeVessel.getAutoPilot().engage(); + // Fazer a nave apontar usando o piloto automático, na marra + while (Math.abs(activeVessel.getAutoPilot().getError()) > 1) { + activeVessel.getAutoPilot().wait_(); + } control.setRCS(true); - // PRIMEIRA PARTE DO DOCKING: APROXIMAÇÃO - positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); - positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); + activeVessel.getAutoPilot().disengage(); + control.setSAS(true); + control.setSASMode(SASMode.STABILITY_ASSIST); - targetDirection = positionMyDockingPort.subtract(positionTargetDockingPort); + // PRIMEIRA PARTE DO DOCKING: APROXIMAÇÃO - double lastYTargetPos = targetDirection.y; - System.out.println(lastYTargetPos + " LAST Y POS"); + Vector targetPosition = new Vector(targetVessel.position(vesselRefFrame)); + double lastXTargetPos = targetPosition.x; + double lastYTargetPos = targetPosition.y; + double lastZTargetPos = targetPosition.z; long sleepTime = 25; + double currentXAxisSpeed = 0; + double currentYAxisSpeed = 0; + double currentZAxisSpeed = 0; while (Math.abs(lastYTargetPos) >= DISTANCE_LIMIT) { - // Calcular distancia: - double distanceBetweenPortsInMeters = positionTargetDockingPort.subtract(positionMyDockingPort) - .magnitude(); - double currentRelativeVelocity = new Vector(activeVessel.velocity( - targetDockingPort.getReferenceFrame())).magnitude(); - // Buscar posições atuais: + targetPosition = new Vector(targetVessel.position(vesselRefFrame)); + + // Atualizar posições para linhas positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); - - targetDirection = positionMyDockingPort.subtract(positionTargetDockingPort); + updateLines(positionMyDockingPort, positionTargetDockingPort); // Calcular velocidade de cada eixo: - double currentYAxisSpeed = (targetDirection.y - lastYTargetPos) * sleepTime; - - // Calcular o valor PID de cada eixo: - float forwardsError = (float) RCSForwardsCtrl.calculate(targetDirection.y, - Math.signum(targetDirection.y) * DISTANCE_LIMIT); - float speedError = (float) speedCtrl.calculate(currentYAxisSpeed / 10, 0); - - control.setForward(forwardsError); - - updateLines(positionMyDockingPort, positionTargetDockingPort); + currentXAxisSpeed = (targetPosition.x - lastXTargetPos) * sleepTime; + currentYAxisSpeed = (targetPosition.y - lastYTargetPos) * sleepTime; + currentZAxisSpeed = (targetPosition.z - lastZTargetPos) * sleepTime; + + // Calcular a aceleração para cada eixo no RCS: + float forwardsError = calculateThrottle(DISTANCE_LIMIT, DISTANCE_LIMIT * 2, currentYAxisSpeed, + targetPosition.y, SPEED_LIMIT); + float sidewaysError = calculateThrottle(0, 10, currentXAxisSpeed, targetPosition.x, SPEED_LIMIT); + float upwardsError = calculateThrottle(0, 10, currentZAxisSpeed, targetPosition.z, SPEED_LIMIT); + control.setForward((float) forwardsError); + control.setRight((float) sidewaysError); + control.setUp((float) -upwardsError); + + // Guardar últimas posições: + lastXTargetPos = targetPosition.x; + lastYTargetPos = targetPosition.y; + lastZTargetPos = targetPosition.z; Thread.sleep(sleepTime); - loopTimeInSeconds += 0.05; - lastYTargetPos = targetDirection.y; } // SEGUNDA PARTE APONTAR PRO LADO CONTRARIO: @@ -142,60 +137,40 @@ public static void main(String[] args) { activeVessel.getAutoPilot().setReferenceFrame(orbitalRefVessel); activeVessel.getAutoPilot().setTargetDirection(direcaoContrariaDockingPortAlvo.toTriplet()); activeVessel.getAutoPilot().setTargetRoll(90); - activeVessel.getAutoPilot().wait_(); + while (Math.abs(activeVessel.getAutoPilot().getError()) > 1) { + activeVessel.getAutoPilot().wait_(); + } activeVessel.getAutoPilot().disengage(); control.setSAS(true); control.setSASMode(SASMode.STABILITY_ASSIST); - Thread.sleep(1000); control.setRCS(true); - positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); - positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); - Vector distanceBetweenPorts = positionMyDockingPort.subtract(positionTargetDockingPort); - loopTimeInSeconds = 0; - double lastXTargetPos = distanceBetweenPorts.x; - lastYTargetPos = distanceBetweenPorts.y; - double lastZTargetPos = distanceBetweenPorts.z; - sleepTime = 50; - while (true) { - // Buscar posições atuais: + while (targetVessel != null) { + targetPosition = new Vector(targetDockingPort.position(vesselRefFrame)); + + // Atualizar posições para linhas positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); - positionTargetDockingPort = new Vector(targetDockingPort.position(vesselRefFrame)); - distanceBetweenPorts = positionMyDockingPort.subtract(positionTargetDockingPort); + updateLines(positionMyDockingPort, targetPosition); // Calcular velocidade de cada eixo: - double currentXAxisSpeed = (distanceBetweenPorts.x - lastXTargetPos) * sleepTime; - double currentYAxisSpeed = (distanceBetweenPorts.y - lastYTargetPos) * sleepTime; - double currentZAxisSpeed = (distanceBetweenPorts.z - lastZTargetPos) * sleepTime; - - // Calcular o valor PID de cada eixo: - float sidewaysError = (float) RCSSidewaysCtrl.calculate(distanceBetweenPorts.x, 0); - float upwardsError = (float) RCSUpwardsCtrl.calculate(distanceBetweenPorts.z, 0); - float forwardsError = (float) RCSForwardsCtrl.calculate(distanceBetweenPorts.y, 0); - float speedError = (float) speedCtrl.calculate(currentYAxisSpeed / 10, 0); - - boolean shouldGetCloser = Math.abs(distanceBetweenPorts.x) < 0.3 - && Math.abs(distanceBetweenPorts.z) < 0.3; - - if (shouldGetCloser) { - control.setForward(forwardsError); - control.setRight(0); - control.setUp(0); - } else { - control.setForward(speedError); - control.setRight(sidewaysError); - control.setUp(-upwardsError); - } - - // Guardando últimas posições: - lastXTargetPos = distanceBetweenPorts.x; - lastYTargetPos = distanceBetweenPorts.y; - lastZTargetPos = distanceBetweenPorts.z; - - adjustPID(sleepTime); - updateLines(positionMyDockingPort, positionTargetDockingPort); - loopTimeInSeconds += 0.05; + currentXAxisSpeed = (targetPosition.x - lastXTargetPos) * sleepTime; + currentYAxisSpeed = (targetPosition.y - lastYTargetPos) * sleepTime; + currentZAxisSpeed = (targetPosition.z - lastZTargetPos) * sleepTime; + + // Calcular a aceleração para cada eixo no RCS: + float forwardsError = calculateThrottle(5, 10, currentYAxisSpeed, + targetPosition.y, SPEED_LIMIT); + float sidewaysError = calculateThrottle(-1, 10, currentXAxisSpeed, targetPosition.x, SPEED_LIMIT); + float upwardsError = calculateThrottle(-1, 10, currentZAxisSpeed, targetPosition.z, SPEED_LIMIT); + control.setForward((float) forwardsError); + control.setRight((float) sidewaysError); + control.setUp((float) -upwardsError); + + // Guardar últimas posições: + lastXTargetPos = targetPosition.x; + lastYTargetPos = targetPosition.y; + lastZTargetPos = targetPosition.z; Thread.sleep(sleepTime); } // // Close the connection when finished @@ -205,31 +180,12 @@ public static void main(String[] args) { } } - // distancia x5 , p: 0.05, i: 0.0001, d: p x2 - private static void adjustPID(double timeSample) { - double ms = 1000 / timeSample; - double pGain = 4 / ms; - double iGain = pGain / 500; - double dGain = pGain * 2; - speedCtrl.setTimeSample(timeSample); - speedCtrl.setPIDValues(pGain, iGain, dGain); - RCSUpwardsCtrl.setTimeSample(timeSample); - RCSUpwardsCtrl.setPIDValues(pGain, iGain, dGain); - RCSSidewaysCtrl.setTimeSample(timeSample); - RCSSidewaysCtrl.setPIDValues(pGain, iGain, dGain); - RCSForwardsCtrl.setTimeSample(timeSample); - RCSForwardsCtrl.setPIDValues(pGain, iGain, dGain); - } - - private static void setDirection(double distance) { - try { - if (distance > DISTANCE_LIMIT) { - control.setSASMode(SASMode.TARGET); - } else { - control.setSASMode(SASMode.STABILITY_ASSIST); - } - } catch (RPCException e) { - } + private static float calculateThrottle(double minDistance, double maxDistance, double currentSpeed, + double currentPosition, double speedLimit) { + double limiter = Utilities.remap(minDistance, maxDistance, 0, 1, Math.abs(currentPosition), true); + double change = (Utilities.remap(-speedLimit, speedLimit, -1.0, 1.0, + currentSpeed + (Math.signum(currentPosition) * (limiter * speedLimit)), true)); + return (float) change; } private static void createLines(Vector start, Vector end) { diff --git a/src/com/pesterenan/controllers/LandingController.java b/src/com/pesterenan/controllers/LandingController.java index 249e73b..1e96712 100644 --- a/src/com/pesterenan/controllers/LandingController.java +++ b/src/com/pesterenan/controllers/LandingController.java @@ -17,6 +17,7 @@ public class LandingController extends Controller { public static final double MAX_VELOCITY = 5; + private static final long sleepTime = 50; private static final double velP = 0.05; private static final double velI = 0.005; private static final double velD = 0.001; @@ -40,8 +41,8 @@ public LandingController(Map commands) { } private void initializeParameters() { - altitudeCtrl = new ControlePID(getSpaceCenter(), 50); - velocityCtrl = new ControlePID(getSpaceCenter(), 50); + altitudeCtrl = new ControlePID(getSpaceCenter(), sleepTime); + velocityCtrl = new ControlePID(getSpaceCenter(), sleepTime); altitudeCtrl.setOutput(0, 1); velocityCtrl.setOutput(0, 1); } @@ -79,7 +80,7 @@ private void hoverArea() { changeControlMode(); } catch (RPCException | StreamException ignored) { } - Thread.sleep(50); + Thread.sleep(sleepTime); } } catch (InterruptedException | RPCException ignored) { // disengageAfterException(Bundle.getString("status_liftoff_abort")); @@ -107,7 +108,7 @@ private void autoLanding() { } getNaveAtual().getControl().setBrakes(true); changeControlMode(); - Thread.sleep(50); + Thread.sleep(sleepTime); } } catch (RPCException | StreamException | InterruptedException e) { setCurrentStatus(Bundle.getString("status_ready")); @@ -132,6 +133,8 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt } break; case APPROACHING: + altitudeCtrl.reset(); + velocityCtrl.reset(); altitudeCtrl.setOutput(0, 1); velocityCtrl.setOutput(0, 1); double currentVelocity = calculateCurrentVelocityMagnitude(); @@ -140,7 +143,7 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt double threshold = Utilities.clamp( ((currentVelocity + zeroVelocity) - landingDistanceThreshold) / landingDistanceThreshold, 0, 1); - altPID = altitudeCtrl.calculate(currentVelocity - zeroVelocity, 0); + altPID = altitudeCtrl.calculate(currentVelocity, zeroVelocity); velPID = velocityCtrl.calculate(velVertical.get(), -Utilities.clamp(altitudeSup.get() * 0.1, 1, 10)); throttle(Utilities.linearInterpolation(velPID, altPID, threshold)); navigation.aimForLanding(); @@ -154,10 +157,11 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt } currentMode = MODE.LANDING; } - System.out.println("altCtrl: " + altPID); setCurrentStatus("Se aproximando do momento do pouso..."); break; case GOING_UP: + altitudeCtrl.reset(); + velocityCtrl.reset(); altitudeCtrl.setOutput(-0.5, 0.5); velocityCtrl.setOutput(-0.5, 0.5); altPID = altitudeCtrl.calculate(altitudeErrorPercentage, HUNDRED_PERCENT); @@ -167,11 +171,15 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt setCurrentStatus("Subindo altitude..."); break; case GOING_DOWN: + altitudeCtrl.reset(); + velocityCtrl.reset(); controlThrottleByMatchingVerticalVelocity(-MAX_VELOCITY); navigation.aimAtRadialOut(); setCurrentStatus("Baixando altitude..."); break; case LANDING: + altitudeCtrl.reset(); + velocityCtrl.reset(); controlThrottleByMatchingVerticalVelocity( velHorizontal.get() > 4 ? 0 : -Utilities.clamp(altitudeSup.get() * 0.1, 1, 10)); navigation.aimForLanding(); @@ -179,6 +187,8 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt hasTheVesselLanded(); break; case HOVERING: + altitudeCtrl.reset(); + velocityCtrl.reset(); altitudeCtrl.setOutput(-0.5, 0.5); velocityCtrl.setOutput(-0.5, 0.5); altPID = altitudeCtrl.calculate(altitudeErrorPercentage, HUNDRED_PERCENT); @@ -207,7 +217,7 @@ private void deOrbitShip() throws RPCException, StreamException, InterruptedExce navigation.aimForLanding(); setCurrentStatus(Bundle.getString("status_orienting_ship")); ap.wait_(); - Thread.sleep(50); + Thread.sleep(sleepTime); } double targetPeriapsis = currentBody.getAtmosphereDepth(); targetPeriapsis = targetPeriapsis > 0 ? targetPeriapsis / 2 : -currentBody.getEquatorialRadius() / 2; @@ -215,7 +225,7 @@ private void deOrbitShip() throws RPCException, StreamException, InterruptedExce navigation.aimForLanding(); throttle(altitudeCtrl.calculate(targetPeriapsis, periastro.get())); setCurrentStatus(Bundle.getString("status_lowering_periapsis")); - Thread.sleep(50); + Thread.sleep(sleepTime); } getNaveAtual().getControl().setRCS(false); throttle(0.0f); @@ -228,14 +238,10 @@ private void deOrbitShip() throws RPCException, StreamException, InterruptedExce private void adjustPIDbyTWR() throws RPCException, StreamException { double currentTWR = Math.min(getTWR(), maxTWR); // double currentTWR = getMaxAcel(maxTWR); - altitudeCtrl.setPIDValues(currentTWR * 0.05, 10, 0.001); - velocityCtrl.setPIDValues(currentTWR * 0.005, 0.1, 0.001); - } - - private double calculateCurrentVelocityMagnitude() throws RPCException, StreamException { - double timeToGround = altitudeSup.get() / velVertical.get(); - double horizontalDistance = velHorizontal.get() * timeToGround; - return calculateEllipticTrajectory(horizontalDistance, altitudeSup.get()); + double pGain = currentTWR / (sleepTime); + System.out.println(pGain); + altitudeCtrl.setPIDValues(pGain * 0.1, 0.0002, pGain * 0.1 * 2); + velocityCtrl.setPIDValues(pGain * 0.1, 0.1, 0.001); } private boolean hasTheVesselLanded() throws RPCException { @@ -254,6 +260,12 @@ private boolean hasTheVesselLanded() throws RPCException { return false; } + private double calculateCurrentVelocityMagnitude() throws RPCException, StreamException { + double timeToGround = altitudeSup.get() / velVertical.get(); + double horizontalDistance = velHorizontal.get() * timeToGround; + return calculateEllipticTrajectory(horizontalDistance, altitudeSup.get()); + } + private double calculateZeroVelocityMagnitude() throws RPCException, StreamException { double zeroVelocityDistance = calculateEllipticTrajectory(velHorizontal.get(), velVertical.get()); double zeroVelocityBurnTime = zeroVelocityDistance / getMaxAcel(maxTWR); diff --git a/src/com/pesterenan/utils/ControlePID.java b/src/com/pesterenan/utils/ControlePID.java index 337f414..51ea12f 100644 --- a/src/com/pesterenan/utils/ControlePID.java +++ b/src/com/pesterenan/utils/ControlePID.java @@ -32,6 +32,14 @@ public ControlePID(double kp, double ki, double kd, double outputMin, double out this.outputMax = outputMax; } + public void reset() { + this.previousError = 0; + this.previousMeasurement = 0; + this.proportionalTerm = 0; + this.integralTerm = 0; + this.derivativeTerm = 0; + } + public double calculate(double measurement, double setPoint) { double now = this.getCurrentTime(); double changeInTime = now - lastTime; From a16771cdc02b71c378b1112c08099e41c7d2af7b Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Tue, 21 Nov 2023 19:57:13 -0300 Subject: [PATCH 16/19] [Docking]: Implement docking into MechPeste --- .../controllers/DockingController.java | 240 ++++++++++++++++++ src/com/pesterenan/model/ActiveVessel.java | 12 +- .../resources/MechPesteBundle.properties | 2 + .../resources/MechPesteBundle_pt.properties | 2 + src/com/pesterenan/utils/Modulos.java | 5 +- src/com/pesterenan/views/DockingJPanel.java | 123 +++++++++ .../views/FunctionsAndTelemetryJPanel.java | 8 +- src/com/pesterenan/views/MainGui.java | 3 + 8 files changed, 389 insertions(+), 6 deletions(-) create mode 100644 src/com/pesterenan/controllers/DockingController.java create mode 100644 src/com/pesterenan/views/DockingJPanel.java diff --git a/src/com/pesterenan/controllers/DockingController.java b/src/com/pesterenan/controllers/DockingController.java new file mode 100644 index 0000000..59bcd0d --- /dev/null +++ b/src/com/pesterenan/controllers/DockingController.java @@ -0,0 +1,240 @@ +package com.pesterenan.controllers; + +import static com.pesterenan.MechPeste.getConnection; +import static com.pesterenan.MechPeste.getSpaceCenter; + +import java.util.Map; + +import com.pesterenan.utils.Modulos; +import com.pesterenan.utils.Utilities; +import com.pesterenan.utils.Vector; +import com.pesterenan.views.StatusJPanel; + +import krpc.client.RPCException; +import krpc.client.services.Drawing; +import krpc.client.services.Drawing.Line; +import krpc.client.services.SpaceCenter.Control; +import krpc.client.services.SpaceCenter.DockingPort; +import krpc.client.services.SpaceCenter.ReferenceFrame; +import krpc.client.services.SpaceCenter.SASMode; +import krpc.client.services.SpaceCenter.Vessel; + +public class DockingController extends Controller { + + private Drawing drawing; + private Vessel targetVessel; + private Control control; + + private ReferenceFrame orbitalRefVessel; + private ReferenceFrame vesselRefFrame; + private ReferenceFrame orbitalRefBody; + private Line distanceLine; + private Line distLineXAxis; + private Line distLineYAxis; + private Line distLineZAxis; + private DockingPort myDockingPort; + private DockingPort targetDockingPort; + private Vector positionMyDockingPort; + private Vector positionTargetDockingPort; + + private final double DISTANCE_LIMIT = 25.0; + private double SPEED_LIMIT = 3.0; + private double currentXAxisSpeed = 0.0; + private double currentYAxisSpeed = 0.0; + private double currentZAxisSpeed = 0.0; + private double lastXTargetPos = 0.0; + private double lastYTargetPos = 0.0; + private double lastZTargetPos = 0.0; + private long sleepTime = 25; + + public DockingController(Map commands) { + super(); + this.commands = commands; + initializeParameters(); + } + + private void initializeParameters() { + try { + SPEED_LIMIT = Double.parseDouble(commands.get(Modulos.VELOCIDADE_MAX.get())); + drawing = Drawing.newInstance(getConnection()); + targetVessel = getSpaceCenter().getTargetVessel(); + vesselRefFrame = getNaveAtual().getReferenceFrame(); + orbitalRefVessel = getNaveAtual().getOrbitalReferenceFrame(); + orbitalRefBody = getNaveAtual().getOrbit().getBody().getReferenceFrame(); + + myDockingPort = getNaveAtual().getParts().getDockingPorts().get(0); + targetDockingPort = targetVessel.getParts().getDockingPorts().get(0); + + positionMyDockingPort = new Vector(myDockingPort.position(orbitalRefVessel)); + positionTargetDockingPort = new Vector(targetDockingPort.position(orbitalRefVessel)); + } catch (RPCException ignored) { + } + } + + @Override + public void run() { + if (commands.get(Modulos.MODULO.get()).equals(Modulos.MODULO_DOCKING.get())) { + startDocking(); + } + } + + public void startDocking() { + try { + // Initialize parameters for the script, connection and setup: + initializeParameters(); + + // Setting up the control + control = getNaveAtual().getControl(); + control.setSAS(true); + control.setRCS(false); + control.setSASMode(SASMode.STABILITY_ASSIST); + + Vector targetDirection = new Vector(getNaveAtual().position(orbitalRefVessel)) + .subtract(new Vector(targetVessel.position(orbitalRefVessel))).multiply(-1); + getNaveAtual().getAutoPilot().setReferenceFrame(orbitalRefVessel); + getNaveAtual().getAutoPilot().setTargetDirection(targetDirection.toTriplet()); + getNaveAtual().getAutoPilot().setTargetRoll(90); + getNaveAtual().getAutoPilot().engage(); + // Fazer a nave apontar usando o piloto automático, na marra + while (Math.abs(getNaveAtual().getAutoPilot().getError()) > 3) { + if (Thread.interrupted()) { + throw new InterruptedException(); + } + Thread.sleep(100); + System.out.println(getNaveAtual().getAutoPilot().getError()); + } + control.setRCS(true); + getNaveAtual().getAutoPilot().disengage(); + control.setSAS(true); + control.setSASMode(SASMode.STABILITY_ASSIST); + createLines(positionMyDockingPort, positionTargetDockingPort); + + // PRIMEIRA PARTE DO DOCKING: APROXIMAÇÃO + + Vector targetPosition = new Vector(targetVessel.position(vesselRefFrame)); + lastXTargetPos = targetPosition.x; + lastYTargetPos = targetPosition.y; + lastZTargetPos = targetPosition.z; + + while (Math.abs(lastYTargetPos) >= DISTANCE_LIMIT) { + if (Thread.interrupted()) { + throw new InterruptedException(); + } + targetPosition = new Vector(targetVessel.position(vesselRefFrame)); + controlShipRCS(targetPosition, DISTANCE_LIMIT); + Thread.sleep(sleepTime); + } + + // SEGUNDA PARTE APONTAR PRO LADO CONTRARIO: + Vector direcaoContrariaDockingPortAlvo = new Vector(targetDockingPort.direction(orbitalRefVessel)) + .multiply(-1); + control.setSAS(false); + control.setRCS(false); + getNaveAtual().getAutoPilot().engage(); + getNaveAtual().getAutoPilot().setReferenceFrame(orbitalRefVessel); + getNaveAtual().getAutoPilot().setTargetDirection(direcaoContrariaDockingPortAlvo.toTriplet()); + getNaveAtual().getAutoPilot().setTargetRoll(90); + + while (Math.abs(getNaveAtual().getAutoPilot().getError()) > 1) { + if (Thread.interrupted()) { + throw new InterruptedException(); + } + Thread.sleep(100); + System.out.println(getNaveAtual().getAutoPilot().getError()); + } + + getNaveAtual().getAutoPilot().disengage(); + control.setSAS(true); + control.setSASMode(SASMode.STABILITY_ASSIST); + Thread.sleep(1000); + control.setRCS(true); + + while (true) { + if (Thread.interrupted()) { + throw new InterruptedException(); + } + targetPosition = new Vector(targetDockingPort.position(vesselRefFrame)); + controlShipRCS(targetPosition, 2); + Thread.sleep(sleepTime); + } + } catch (RPCException | InterruptedException e) { + StatusJPanel.setStatusMessage("Docking aborted."); + } + } + + private void controlShipRCS(Vector targetPosition, double distanceLimit) { + try { + // Atualizar posições para linhas + positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); + updateLines(positionMyDockingPort, targetPosition); + + // Calcular velocidade de cada eixo: + currentXAxisSpeed = (targetPosition.x - lastXTargetPos) * sleepTime; + currentYAxisSpeed = (targetPosition.y - lastYTargetPos) * sleepTime; + currentZAxisSpeed = (targetPosition.z - lastZTargetPos) * sleepTime; + + // Calcular a aceleração para cada eixo no RCS: + float forwardsError = calculateThrottle(distanceLimit, distanceLimit * 2, currentYAxisSpeed, + targetPosition.y, SPEED_LIMIT); + float sidewaysError = calculateThrottle(0, 10, currentXAxisSpeed, targetPosition.x, SPEED_LIMIT); + float upwardsError = calculateThrottle(0, 10, currentZAxisSpeed, targetPosition.z, SPEED_LIMIT); + control.setForward((float) forwardsError); + control.setRight((float) sidewaysError); + control.setUp((float) -upwardsError); + + // Guardar últimas posições: + lastXTargetPos = targetPosition.x; + lastYTargetPos = targetPosition.y; + lastZTargetPos = targetPosition.z; + } catch (RPCException ignored) { + } + } + + private float calculateThrottle(double minDistance, double maxDistance, double currentSpeed, + double currentPosition, double speedLimit) { + double limiter = Utilities.remap(minDistance, maxDistance, 0, 1, Math.abs(currentPosition), true); + double change = (Utilities.remap(-speedLimit, speedLimit, -1.0, 1.0, + currentSpeed + (Math.signum(currentPosition) * (limiter * speedLimit)), true)); + return (float) change; + } + + private void createLines(Vector start, Vector end) { + try { + distanceLine = drawing.addLine(start.toTriplet(), + end.toTriplet(), vesselRefFrame, true); + distLineXAxis = drawing.addLine(start.toTriplet(), + new Vector(end.x, 0.0, 0.0).toTriplet(), + vesselRefFrame, true); + distLineYAxis = drawing.addLine(start.toTriplet(), + new Vector(end.x, end.y, 0.0).toTriplet(), + vesselRefFrame, true); + distLineZAxis = drawing.addLine(start.toTriplet(), + end.toTriplet(), + vesselRefFrame, true); + distanceLine.setThickness(0.5f); + distLineXAxis.setThickness(0.25f); + distLineYAxis.setThickness(0.25f); + distLineZAxis.setThickness(0.25f); + distLineXAxis.setColor(new Vector(1.0, 0.0, 0.0).toTriplet()); + distLineYAxis.setColor(new Vector(0.0, 1.0, 0.0).toTriplet()); + distLineZAxis.setColor(new Vector(0.0, 0.0, 1.0).toTriplet()); + } catch (RPCException e) { + } + } + + private void updateLines(Vector start, Vector end) { + // Updating drawing lines: + try { + distanceLine.setStart(start.toTriplet()); + distanceLine.setEnd(end.toTriplet()); + distLineXAxis.setStart(start.toTriplet()); + distLineXAxis.setEnd(new Vector(end.x, 0.0, 0.0).toTriplet()); + distLineYAxis.setStart(distLineXAxis.getEnd()); + distLineYAxis.setEnd(new Vector(end.x, end.y, 0.0).toTriplet()); + distLineZAxis.setStart(distLineYAxis.getEnd()); + distLineZAxis.setEnd(end.toTriplet()); + } catch (RPCException e) { + } + } + +} diff --git a/src/com/pesterenan/model/ActiveVessel.java b/src/com/pesterenan/model/ActiveVessel.java index f01f206..99efa65 100644 --- a/src/com/pesterenan/model/ActiveVessel.java +++ b/src/com/pesterenan/model/ActiveVessel.java @@ -6,6 +6,7 @@ import com.pesterenan.controllers.LiftoffController; import com.pesterenan.controllers.ManeuverController; import com.pesterenan.controllers.RoverController; +import com.pesterenan.controllers.DockingController; import com.pesterenan.resources.Bundle; import com.pesterenan.utils.Modulos; import com.pesterenan.utils.Telemetry; @@ -190,6 +191,11 @@ public void startModule(Map commands) { controller = new RoverController(commands); runningModule = true; } + if (currentFunction.equals(Modulos.MODULO_DOCKING.get())) { + controller = new DockingController(commands); + System.out.println("escolheu modulo docking"); + runningModule = true; + } controllerThread = new Thread(controller, currentVesselId + " - " + currentFunction); controllerThread.start(); } @@ -227,7 +233,7 @@ public void cancelControl() { } } - public boolean hasModuleRunning() { - return runningModule; - } + public boolean hasModuleRunning() { + return runningModule; + } } diff --git a/src/com/pesterenan/resources/MechPesteBundle.properties b/src/com/pesterenan/resources/MechPesteBundle.properties index 6035da3..03fcb5a 100644 --- a/src/com/pesterenan/resources/MechPesteBundle.properties +++ b/src/com/pesterenan/resources/MechPesteBundle.properties @@ -3,6 +3,7 @@ btn_func_liftoff = Orbital Liftoff btn_func_create_maneuver = Create Maneuvers btn_func_maneuver = Maneuvers btn_func_rover = Drive Rover +btn_func_docking = Docking btn_stat_connect = Connect installer_dialog_btn_browse = Browse... installer_dialog_btn_cancel = Cancel @@ -62,6 +63,7 @@ pnl_mnv_lbl_exec_mnv = Execute next maneuver\: pnl_rover_border = Drive Rover\: pnl_rover_btn_back = Back pnl_rover_btn_drive = Drive +pnl_rover_btn_docking = Start Docking pnl_rover_default_name = Target pnl_rover_lbl_max_speed = Max Speed\: pnl_rover_max_speed_above_3 = Max speed must be over 3m/s\u00B2 diff --git a/src/com/pesterenan/resources/MechPesteBundle_pt.properties b/src/com/pesterenan/resources/MechPesteBundle_pt.properties index fbace8b..36cb87a 100644 --- a/src/com/pesterenan/resources/MechPesteBundle_pt.properties +++ b/src/com/pesterenan/resources/MechPesteBundle_pt.properties @@ -3,6 +3,7 @@ btn_func_liftoff = Decolagem Orbital btn_func_create_maneuver = Criar Manobras btn_func_maneuver = Manobras btn_func_rover = Pilotar Rover +btn_func_docking = Acoplagem btn_stat_connect = Conectar installer_dialog_btn_browse = Escolher... installer_dialog_btn_cancel = Cancelar @@ -61,6 +62,7 @@ pnl_mnv_lbl_exec_mnv = Executar pr\u00F3xima
manobra\: pnl_rover_border = Pilotar Rover\: pnl_rover_btn_back = Voltar pnl_rover_btn_drive = Pilotar +pnl_rover_btn_docking = Iniciar Acoplagem pnl_rover_default_name = Alvo pnl_rover_lbl_max_speed = Velocidade m\u00E1xima: pnl_rover_max_speed_above_3 = A velocidade m\u00E1xima tem que ser acima de 3m/s\u00b2 diff --git a/src/com/pesterenan/utils/Modulos.java b/src/com/pesterenan/utils/Modulos.java index 5bdf85c..f31f409 100644 --- a/src/com/pesterenan/utils/Modulos.java +++ b/src/com/pesterenan/utils/Modulos.java @@ -22,18 +22,19 @@ public enum Modulos { MODULO_POUSO_SOBREVOAR("HOVER"), MODULO_POUSO("LANDING"), MODULO_ROVER("ROVER"), + MODULO_DOCKING("DOCKING"), MODULO_TELEMETRIA("TELEMETRY"), MODULO("Módulo"), NAVE_ALVO("Nave alvo"), NOME_MARCADOR("Nome do marcador"), ORBITA_BAIXA("ÓRBITA BAIXA"), PERIASTRO("Periastro"), - POUSAR("Pousar nave"), + POUSAR("Pousar nave"), QUADRATICA("Quadrática"), RENDEZVOUS("Rendezvous"), ROLAGEM("Rolagem"), SINUSOIDAL("Sinusoidal"), - SOBREVOO_POS_POUSO("SOBREVOO PÓS POUSO"), + SOBREVOO_POS_POUSO("SOBREVOO PÓS POUSO"), TIPO_ALVO_ROVER("Tipo de Alvo do Rover"), USAR_ESTAGIOS("Usar Estágios"), VELOCIDADE_MAX("Velocidade Máxima"); diff --git a/src/com/pesterenan/views/DockingJPanel.java b/src/com/pesterenan/views/DockingJPanel.java new file mode 100644 index 0000000..8c5e975 --- /dev/null +++ b/src/com/pesterenan/views/DockingJPanel.java @@ -0,0 +1,123 @@ +package com.pesterenan.views; + +import com.pesterenan.MechPeste; +import com.pesterenan.resources.Bundle; +import com.pesterenan.utils.Modulos; + +import javax.swing.*; +import javax.swing.border.EtchedBorder; +import javax.swing.border.TitledBorder; +import java.awt.*; +import java.awt.event.ActionEvent; +import java.util.HashMap; +import java.util.Map; + +import static com.pesterenan.views.MainGui.BTN_DIMENSION; +import static com.pesterenan.views.MainGui.MARGIN_BORDER_10_PX_LR; +import static com.pesterenan.views.MainGui.PNL_DIMENSION; + +public class DockingJPanel extends JPanel implements UIMethods { + + private static final long serialVersionUID = 0L; + + private JLabel lblMaxSpeed; + private JTextField txfMaxSpeed; + private JButton btnBack, btnStartDocking; + + public DockingJPanel() { + initComponents(); + setupComponents(); + layoutComponents(); + } + + @Override + public void initComponents() { + // Labels: + lblMaxSpeed = new JLabel(Bundle.getString("pnl_rover_lbl_max_speed")); + + // Textfields: + txfMaxSpeed = new JTextField("3"); + + // Buttons: + btnBack = new JButton(Bundle.getString("pnl_rover_btn_back")); + btnStartDocking = new JButton(Bundle.getString("pnl_rover_btn_docking")); + } + + @Override + public void setupComponents() { + // Main Panel setup: + setBorder(new TitledBorder(null, Bundle.getString("btn_func_docking"))); + + // Setting-up components: + txfMaxSpeed.setHorizontalAlignment(SwingConstants.RIGHT); + txfMaxSpeed.setMaximumSize(BTN_DIMENSION); + txfMaxSpeed.setPreferredSize(BTN_DIMENSION); + + btnBack.addActionListener(MainGui::backToTelemetry); + btnBack.setMaximumSize(BTN_DIMENSION); + btnBack.setPreferredSize(BTN_DIMENSION); + btnStartDocking.addActionListener(this::handleStartDocking); + btnStartDocking.setMaximumSize(BTN_DIMENSION); + btnStartDocking.setPreferredSize(BTN_DIMENSION); + } + + @Override + public void layoutComponents() { + // Main Panel layout: + setPreferredSize(PNL_DIMENSION); + setSize(PNL_DIMENSION); + setLayout(new BorderLayout()); + + // Layout components: + JPanel pnlMaxSpeed = new JPanel(); + pnlMaxSpeed.setLayout(new BoxLayout(pnlMaxSpeed, BoxLayout.X_AXIS)); + pnlMaxSpeed.add(lblMaxSpeed); + pnlMaxSpeed.add(Box.createHorizontalGlue()); + pnlMaxSpeed.add(txfMaxSpeed); + + JPanel pnlRoverControls = new JPanel(); + pnlRoverControls.setLayout(new BoxLayout(pnlRoverControls, BoxLayout.Y_AXIS)); + pnlRoverControls.setBorder(MARGIN_BORDER_10_PX_LR); + pnlRoverControls.add(MainGui.createMarginComponent(0, 6)); + pnlRoverControls.add(pnlMaxSpeed); + pnlRoverControls.add(Box.createVerticalGlue()); + + JPanel pnlButtons = new JPanel(); + pnlButtons.setLayout(new BoxLayout(pnlButtons, BoxLayout.X_AXIS)); + pnlButtons.add(btnStartDocking); + pnlButtons.add(Box.createHorizontalGlue()); + pnlButtons.add(btnBack); + + JPanel pnlMain = new JPanel(); + pnlMain.setLayout(new BoxLayout(pnlMain, BoxLayout.X_AXIS)); + pnlRoverControls.setAlignmentY(TOP_ALIGNMENT); + pnlMain.add(pnlRoverControls); + + setLayout(new BorderLayout()); + add(pnlMain, BorderLayout.CENTER); + add(pnlButtons, BorderLayout.SOUTH); + } + + private void handleStartDocking(ActionEvent e) { + System.out.println("chamou startdocking"); + Map commands = new HashMap<>(); + commands.put(Modulos.MODULO.get(), Modulos.MODULO_DOCKING.get()); + commands.put(Modulos.VELOCIDADE_MAX.get(), txfMaxSpeed.getText()); + MechPeste.newInstance().startModule(commands); + } + + private boolean validateTextFields() { + try { + if (Float.parseFloat(txfMaxSpeed.getText()) > 10) { + throw new NumberFormatException(); + } + } catch (NumberFormatException e) { + StatusJPanel.setStatusMessage(Bundle.getString("pnl_rover_max_speed_above_3")); + return false; + } catch (IllegalArgumentException e) { + StatusJPanel.setStatusMessage(Bundle.getString("pnl_rover_waypoint_name_not_empty")); + return false; + } + return true; + } +} diff --git a/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java b/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java index 3f0d9ac..5a6dc03 100644 --- a/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java +++ b/src/com/pesterenan/views/FunctionsAndTelemetryJPanel.java @@ -21,7 +21,7 @@ public class FunctionsAndTelemetryJPanel extends JPanel implements UIMethods { private static final long serialVersionUID = 0L; private final Dimension btnFuncDimension = new Dimension(140, 25); - private JButton btnLiftoff, btnLanding, btnManeuvers, btnRover, btnCancel; + private JButton btnLiftoff, btnLanding, btnManeuvers, btnDocking, btnRover, btnCancel; private static JLabel lblAltitude, lblSurfaceAlt, lblApoapsis, lblPeriapsis, lblVertSpeed, lblHorzSpeed; private static JLabel lblAltitudeValue, lblSurfaceAltValue, lblApoapsisValue; private static JLabel lblPeriapsisValue, lblVertSpeedValue, lblHorzSpeedValue; @@ -53,6 +53,7 @@ public void initComponents() { btnLanding = new JButton(Bundle.getString("btn_func_landing")); btnManeuvers = new JButton(Bundle.getString("btn_func_maneuver")); btnRover = new JButton(Bundle.getString("btn_func_rover")); + btnDocking = new JButton(Bundle.getString("btn_func_docking")); btnCancel = new JButton(Bundle.getString("pnl_tel_btn_cancel")); } @@ -83,6 +84,10 @@ public void setupComponents() { btnRover.setActionCommand(Modulos.MODULO_ROVER.get()); btnRover.setMaximumSize(btnFuncDimension); btnRover.setPreferredSize(btnFuncDimension); + btnDocking.addActionListener(this::changeFunctionPanel); + btnDocking.setActionCommand(Modulos.MODULO_DOCKING.get()); + btnDocking.setMaximumSize(btnFuncDimension); + btnDocking.setPreferredSize(btnFuncDimension); } @Override @@ -94,6 +99,7 @@ public void layoutComponents() { pnlFunctionControls.add(btnLiftoff); pnlFunctionControls.add(btnLanding); pnlFunctionControls.add(btnRover); + pnlFunctionControls.add(btnDocking); pnlFunctionControls.add(btnManeuvers); pnlFunctionControls.add(Box.createVerticalGlue()); diff --git a/src/com/pesterenan/views/MainGui.java b/src/com/pesterenan/views/MainGui.java index 2eab916..f824d7f 100644 --- a/src/com/pesterenan/views/MainGui.java +++ b/src/com/pesterenan/views/MainGui.java @@ -33,6 +33,7 @@ public class MainGui extends JFrame implements ActionListener, UIMethods { private CreateManeuverJPanel pnlCreateManeuvers; private RunManeuverJPanel pnlRunManeuvers; private RoverJPanel pnlRover; + private DockingJPanel pnlDocking; private MainGui() { initComponents(); @@ -75,6 +76,7 @@ public void initComponents() { pnlCreateManeuvers = new CreateManeuverJPanel(); pnlRunManeuvers = new RunManeuverJPanel(); pnlRover = new RoverJPanel(); + pnlDocking = new DockingJPanel(); pnlStatus = new StatusJPanel(); } @@ -130,6 +132,7 @@ public void layoutComponents() { cardJPanels.add(pnlLanding, Modulos.MODULO_POUSO.get()); cardJPanels.add(pnlManeuverJTabbedPane, Modulos.MODULE_MANEUVER.get()); cardJPanels.add(pnlRover, Modulos.MODULO_ROVER.get()); + cardJPanels.add(pnlDocking, Modulos.MODULO_DOCKING.get()); } public void actionPerformed(ActionEvent e) { From 17364110af4a2087b40649f6a79e62e5edb8b9ab Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Wed, 22 Nov 2023 00:38:28 -0300 Subject: [PATCH 17/19] [Docking]: Organize docking code --- .../controllers/DockingController.java | 183 ++++++++++++------ 1 file changed, 122 insertions(+), 61 deletions(-) diff --git a/src/com/pesterenan/controllers/DockingController.java b/src/com/pesterenan/controllers/DockingController.java index 59bcd0d..025bfdd 100644 --- a/src/com/pesterenan/controllers/DockingController.java +++ b/src/com/pesterenan/controllers/DockingController.java @@ -46,6 +46,7 @@ public class DockingController extends Controller { private double lastYTargetPos = 0.0; private double lastZTargetPos = 0.0; private long sleepTime = 25; + private DOCKING_STEPS dockingStep; public DockingController(Map commands) { super(); @@ -58,6 +59,7 @@ private void initializeParameters() { SPEED_LIMIT = Double.parseDouble(commands.get(Modulos.VELOCIDADE_MAX.get())); drawing = Drawing.newInstance(getConnection()); targetVessel = getSpaceCenter().getTargetVessel(); + control = getNaveAtual().getControl(); vesselRefFrame = getNaveAtual().getReferenceFrame(); orbitalRefVessel = getNaveAtual().getOrbitalReferenceFrame(); orbitalRefBody = getNaveAtual().getOrbit().getBody().getReferenceFrame(); @@ -78,74 +80,69 @@ public void run() { } } + private void pointToTarget(Vector targetDirection) throws RPCException, InterruptedException { + getNaveAtual().getAutoPilot().setReferenceFrame(orbitalRefVessel); + getNaveAtual().getAutoPilot().setTargetDirection(targetDirection.toTriplet()); + getNaveAtual().getAutoPilot().setTargetRoll(90); + getNaveAtual().getAutoPilot().engage(); + // Fazer a nave apontar usando o piloto automático, na marra + while (Math.abs(getNaveAtual().getAutoPilot().getError()) > 3) { + if (Thread.interrupted()) { + throw new InterruptedException(); + } + Thread.sleep(100); + System.out.println(getNaveAtual().getAutoPilot().getError()); + } + getNaveAtual().getAutoPilot().disengage(); + control.setSAS(true); + control.setSASMode(SASMode.STABILITY_ASSIST); + } + + private void getCloserToTarget(Vector targetPosition) throws InterruptedException, RPCException { + lastXTargetPos = targetPosition.x; + lastYTargetPos = targetPosition.y; + lastZTargetPos = targetPosition.z; + + while (Math.abs(lastYTargetPos) >= DISTANCE_LIMIT) { + if (Thread.interrupted()) { + throw new InterruptedException(); + } + targetPosition = new Vector(targetVessel.position(vesselRefFrame)); + controlShipRCS(targetPosition, DISTANCE_LIMIT); + Thread.sleep(sleepTime); + } + + } + public void startDocking() { try { - // Initialize parameters for the script, connection and setup: - initializeParameters(); - // Setting up the control - control = getNaveAtual().getControl(); control.setSAS(true); control.setRCS(false); control.setSASMode(SASMode.STABILITY_ASSIST); - - Vector targetDirection = new Vector(getNaveAtual().position(orbitalRefVessel)) - .subtract(new Vector(targetVessel.position(orbitalRefVessel))).multiply(-1); - getNaveAtual().getAutoPilot().setReferenceFrame(orbitalRefVessel); - getNaveAtual().getAutoPilot().setTargetDirection(targetDirection.toTriplet()); - getNaveAtual().getAutoPilot().setTargetRoll(90); - getNaveAtual().getAutoPilot().engage(); - // Fazer a nave apontar usando o piloto automático, na marra - while (Math.abs(getNaveAtual().getAutoPilot().getError()) > 3) { - if (Thread.interrupted()) { - throw new InterruptedException(); - } - Thread.sleep(100); - System.out.println(getNaveAtual().getAutoPilot().getError()); - } - control.setRCS(true); - getNaveAtual().getAutoPilot().disengage(); - control.setSAS(true); - control.setSASMode(SASMode.STABILITY_ASSIST); createLines(positionMyDockingPort, positionTargetDockingPort); // PRIMEIRA PARTE DO DOCKING: APROXIMAÇÃO - Vector targetPosition = new Vector(targetVessel.position(vesselRefFrame)); - lastXTargetPos = targetPosition.x; - lastYTargetPos = targetPosition.y; - lastZTargetPos = targetPosition.z; + if (targetPosition.magnitude() > DISTANCE_LIMIT) { + // Apontar para o alvo: + Vector targetDirection = new Vector(getNaveAtual().position(orbitalRefVessel)) + .subtract(new Vector(targetVessel.position(orbitalRefVessel))).multiply(-1); + pointToTarget(targetDirection); - while (Math.abs(lastYTargetPos) >= DISTANCE_LIMIT) { - if (Thread.interrupted()) { - throw new InterruptedException(); - } - targetPosition = new Vector(targetVessel.position(vesselRefFrame)); - controlShipRCS(targetPosition, DISTANCE_LIMIT); - Thread.sleep(sleepTime); + control.setRCS(true); + + getCloserToTarget(targetPosition); } - // SEGUNDA PARTE APONTAR PRO LADO CONTRARIO: - Vector direcaoContrariaDockingPortAlvo = new Vector(targetDockingPort.direction(orbitalRefVessel)) - .multiply(-1); control.setSAS(false); control.setRCS(false); - getNaveAtual().getAutoPilot().engage(); - getNaveAtual().getAutoPilot().setReferenceFrame(orbitalRefVessel); - getNaveAtual().getAutoPilot().setTargetDirection(direcaoContrariaDockingPortAlvo.toTriplet()); - getNaveAtual().getAutoPilot().setTargetRoll(90); - while (Math.abs(getNaveAtual().getAutoPilot().getError()) > 1) { - if (Thread.interrupted()) { - throw new InterruptedException(); - } - Thread.sleep(100); - System.out.println(getNaveAtual().getAutoPilot().getError()); - } + // SEGUNDA PARTE FICAR DE FRENTE COM A DOCKING PORT: + Vector targetDockingPortDirection = new Vector(targetDockingPort.direction(orbitalRefVessel)) + .multiply(-1); + pointToTarget(targetDockingPortDirection); - getNaveAtual().getAutoPilot().disengage(); - control.setSAS(true); - control.setSASMode(SASMode.STABILITY_ASSIST); Thread.sleep(1000); control.setRCS(true); @@ -153,15 +150,53 @@ public void startDocking() { if (Thread.interrupted()) { throw new InterruptedException(); } - targetPosition = new Vector(targetDockingPort.position(vesselRefFrame)); + targetPosition = new Vector(targetDockingPort.position(vesselRefFrame)) + .subtract(new Vector(myDockingPort.position(vesselRefFrame))); controlShipRCS(targetPosition, 2); Thread.sleep(sleepTime); } - } catch (RPCException | InterruptedException e) { + } catch (RPCException | InterruptedException | IllegalArgumentException e) { StatusJPanel.setStatusMessage("Docking aborted."); } } + /* + * Possibilidades do docking: + * primeiro: a nave ta na orientação certa, e só precisa seguir em frente X e Z + * = 0, Y positivo + * segundo: a nave ta na orientação certa, mas precisa corrigir a posição X e Z, + * Y positivo + * terceiro: a nave está atrás da docking port, precisa corrigir Y primeiro, Y + * negativo + * quarto: a nave está atrás da docking port, precisa afastar X e Z longe da + * nave primeiro, Y negativo + */ + + private enum DOCKING_STEPS { + APPROACH, CORRECT_SIDEWAYS, CORRECT_UPWARDS, CORRECT_FORWARDS + } + + private DOCKING_STEPS checkDockingStep(Vector targetPosition) { + double sidewaysDistance = Math.abs(targetPosition.x); + double upwardsDistance = Math.abs(targetPosition.z); + boolean isInFrontOfTarget = Math.signum(targetPosition.y) == 1; + boolean isOnTheBackOfTarget = Math.signum(targetPosition.y) == -1; + + if (isInFrontOfTarget && sidewaysDistance < 5 && upwardsDistance < 5) { + return DOCKING_STEPS.APPROACH; + } + if (isOnTheBackOfTarget) { + return DOCKING_STEPS.CORRECT_FORWARDS; + } + if (isInFrontOfTarget && sidewaysDistance > 5) { + return DOCKING_STEPS.CORRECT_SIDEWAYS; + } + if (isInFrontOfTarget && upwardsDistance > 5) { + return DOCKING_STEPS.CORRECT_UPWARDS; + } + return DOCKING_STEPS.APPROACH; + } + private void controlShipRCS(Vector targetPosition, double distanceLimit) { try { // Atualizar posições para linhas @@ -173,14 +208,40 @@ private void controlShipRCS(Vector targetPosition, double distanceLimit) { currentYAxisSpeed = (targetPosition.y - lastYTargetPos) * sleepTime; currentZAxisSpeed = (targetPosition.z - lastZTargetPos) * sleepTime; - // Calcular a aceleração para cada eixo no RCS: - float forwardsError = calculateThrottle(distanceLimit, distanceLimit * 2, currentYAxisSpeed, - targetPosition.y, SPEED_LIMIT); - float sidewaysError = calculateThrottle(0, 10, currentXAxisSpeed, targetPosition.x, SPEED_LIMIT); - float upwardsError = calculateThrottle(0, 10, currentZAxisSpeed, targetPosition.z, SPEED_LIMIT); - control.setForward((float) forwardsError); - control.setRight((float) sidewaysError); - control.setUp((float) -upwardsError); + dockingStep = checkDockingStep(targetPosition); + float forwardsError, upwardsError, sidewaysError = 0; + switch (dockingStep) { + case APPROACH: + // Calcular a aceleração para cada eixo no RCS: + forwardsError = calculateThrottle(distanceLimit, distanceLimit * 2, currentYAxisSpeed, + targetPosition.y, SPEED_LIMIT); + sidewaysError = calculateThrottle(0, 5, currentXAxisSpeed, targetPosition.x, SPEED_LIMIT); + upwardsError = calculateThrottle(0, 5, currentZAxisSpeed, targetPosition.z, SPEED_LIMIT); + control.setForward((float) forwardsError); + control.setRight((float) sidewaysError); + control.setUp((float) -upwardsError); + break; + case CORRECT_SIDEWAYS: + sidewaysError = calculateThrottle(0, 10, currentXAxisSpeed, targetPosition.x, SPEED_LIMIT); + control.setForward(0); + control.setRight((float) sidewaysError); + control.setUp(0); + break; + case CORRECT_UPWARDS: + upwardsError = calculateThrottle(0, 10, currentZAxisSpeed, targetPosition.z, SPEED_LIMIT); + control.setForward(0); + control.setRight(0); + control.setUp((float) -upwardsError); + break; + case CORRECT_FORWARDS: + forwardsError = calculateThrottle(-10, 20, currentYAxisSpeed, + targetPosition.y, SPEED_LIMIT); + control.setForward((float) forwardsError); + control.setRight(0); + control.setUp(0); + break; + } + System.out.println(dockingStep); // Guardar últimas posições: lastXTargetPos = targetPosition.x; From cd60595827104099773a3f488aeb82a7294e2e9d Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Wed, 22 Nov 2023 16:32:54 -0300 Subject: [PATCH 18/19] [Docking]: Implement maneuvering around target --- .../controllers/DockingController.java | 64 +++++++++---------- 1 file changed, 30 insertions(+), 34 deletions(-) diff --git a/src/com/pesterenan/controllers/DockingController.java b/src/com/pesterenan/controllers/DockingController.java index 025bfdd..eaea4b7 100644 --- a/src/com/pesterenan/controllers/DockingController.java +++ b/src/com/pesterenan/controllers/DockingController.java @@ -145,14 +145,17 @@ public void startDocking() { Thread.sleep(1000); control.setRCS(true); - + double safeDistance = 10; while (true) { if (Thread.interrupted()) { throw new InterruptedException(); } targetPosition = new Vector(targetDockingPort.position(vesselRefFrame)) .subtract(new Vector(myDockingPort.position(vesselRefFrame))); - controlShipRCS(targetPosition, 2); + if (targetPosition.magnitude() < safeDistance) { + safeDistance = 1; + } + controlShipRCS(targetPosition, safeDistance); Thread.sleep(sleepTime); } } catch (RPCException | InterruptedException | IllegalArgumentException e) { @@ -173,31 +176,25 @@ public void startDocking() { */ private enum DOCKING_STEPS { - APPROACH, CORRECT_SIDEWAYS, CORRECT_UPWARDS, CORRECT_FORWARDS + APPROACH, LINE_UP_WITH_TARGET, GO_IN_FRONT_OF_TARGET } - private DOCKING_STEPS checkDockingStep(Vector targetPosition) { + private DOCKING_STEPS checkDockingStep(Vector targetPosition, double forwardsDistanceLimit) { double sidewaysDistance = Math.abs(targetPosition.x); double upwardsDistance = Math.abs(targetPosition.z); boolean isInFrontOfTarget = Math.signum(targetPosition.y) == 1; - boolean isOnTheBackOfTarget = Math.signum(targetPosition.y) == -1; + boolean isOnTheBackOfTarget = Math.signum(targetPosition.y) == -1 && targetPosition.y < forwardsDistanceLimit; - if (isInFrontOfTarget && sidewaysDistance < 5 && upwardsDistance < 5) { - return DOCKING_STEPS.APPROACH; - } if (isOnTheBackOfTarget) { - return DOCKING_STEPS.CORRECT_FORWARDS; + return DOCKING_STEPS.GO_IN_FRONT_OF_TARGET; } - if (isInFrontOfTarget && sidewaysDistance > 5) { - return DOCKING_STEPS.CORRECT_SIDEWAYS; - } - if (isInFrontOfTarget && upwardsDistance > 5) { - return DOCKING_STEPS.CORRECT_UPWARDS; + if (isInFrontOfTarget && (sidewaysDistance > 5 || upwardsDistance > 5)) { + return DOCKING_STEPS.LINE_UP_WITH_TARGET; } return DOCKING_STEPS.APPROACH; } - private void controlShipRCS(Vector targetPosition, double distanceLimit) { + private void controlShipRCS(Vector targetPosition, double forwardsDistanceLimit) { try { // Atualizar posições para linhas positionMyDockingPort = new Vector(myDockingPort.position(vesselRefFrame)); @@ -208,37 +205,36 @@ private void controlShipRCS(Vector targetPosition, double distanceLimit) { currentYAxisSpeed = (targetPosition.y - lastYTargetPos) * sleepTime; currentZAxisSpeed = (targetPosition.z - lastZTargetPos) * sleepTime; - dockingStep = checkDockingStep(targetPosition); + dockingStep = checkDockingStep(targetPosition, forwardsDistanceLimit); float forwardsError, upwardsError, sidewaysError = 0; switch (dockingStep) { case APPROACH: // Calcular a aceleração para cada eixo no RCS: - forwardsError = calculateThrottle(distanceLimit, distanceLimit * 2, currentYAxisSpeed, + forwardsError = calculateThrottle(forwardsDistanceLimit, forwardsDistanceLimit * 3, currentYAxisSpeed, targetPosition.y, SPEED_LIMIT); sidewaysError = calculateThrottle(0, 5, currentXAxisSpeed, targetPosition.x, SPEED_LIMIT); upwardsError = calculateThrottle(0, 5, currentZAxisSpeed, targetPosition.z, SPEED_LIMIT); - control.setForward((float) forwardsError); - control.setRight((float) sidewaysError); - control.setUp((float) -upwardsError); + control.setForward(forwardsError); + control.setRight(sidewaysError); + control.setUp(-upwardsError); break; - case CORRECT_SIDEWAYS: + case LINE_UP_WITH_TARGET: + forwardsError = calculateThrottle(forwardsDistanceLimit, forwardsDistanceLimit * 3, currentYAxisSpeed, + targetPosition.y, 0); sidewaysError = calculateThrottle(0, 10, currentXAxisSpeed, targetPosition.x, SPEED_LIMIT); - control.setForward(0); - control.setRight((float) sidewaysError); - control.setUp(0); - break; - case CORRECT_UPWARDS: upwardsError = calculateThrottle(0, 10, currentZAxisSpeed, targetPosition.z, SPEED_LIMIT); - control.setForward(0); - control.setRight(0); - control.setUp((float) -upwardsError); + control.setForward(forwardsError); + control.setRight(sidewaysError); + control.setUp(-upwardsError); break; - case CORRECT_FORWARDS: - forwardsError = calculateThrottle(-10, 20, currentYAxisSpeed, + case GO_IN_FRONT_OF_TARGET: + forwardsError = calculateThrottle(-20, -10, currentYAxisSpeed, targetPosition.y, SPEED_LIMIT); - control.setForward((float) forwardsError); - control.setRight(0); - control.setUp(0); + sidewaysError = calculateThrottle(0, 5, currentXAxisSpeed, targetPosition.x, 0); + upwardsError = calculateThrottle(0, 5, currentZAxisSpeed, targetPosition.z, 0); + control.setForward(forwardsError); + control.setRight(sidewaysError); + control.setUp(-upwardsError); break; } System.out.println(dockingStep); From 99b7ceef13c01ba4fa8165b56185bcf948e2cc2a Mon Sep 17 00:00:00 2001 From: Pesterenan Date: Fri, 24 Nov 2023 23:04:19 -0300 Subject: [PATCH 19/19] [Landing]: Fix landing speed calculation --- src/com/pesterenan/controllers/LandingController.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/com/pesterenan/controllers/LandingController.java b/src/com/pesterenan/controllers/LandingController.java index 1e96712..df9072c 100644 --- a/src/com/pesterenan/controllers/LandingController.java +++ b/src/com/pesterenan/controllers/LandingController.java @@ -143,11 +143,12 @@ private void changeControlMode() throws RPCException, StreamException, Interrupt double threshold = Utilities.clamp( ((currentVelocity + zeroVelocity) - landingDistanceThreshold) / landingDistanceThreshold, 0, 1); - altPID = altitudeCtrl.calculate(currentVelocity, zeroVelocity); - velPID = velocityCtrl.calculate(velVertical.get(), -Utilities.clamp(altitudeSup.get() * 0.1, 1, 10)); + altPID = altitudeCtrl.calculate(currentVelocity / sleepTime, zeroVelocity / sleepTime); + velPID = velocityCtrl.calculate(velVertical.get() / sleepTime, + (-Utilities.clamp(altitudeSup.get() * 0.1, 3, 20) / sleepTime)); throttle(Utilities.linearInterpolation(velPID, altPID, threshold)); navigation.aimForLanding(); - if (threshold < 0.10 || altitudeSup.get() < landingDistanceThreshold) { + if (threshold < 0.15 || altitudeSup.get() < landingDistanceThreshold) { hoverAltitude = landingDistanceThreshold; getNaveAtual().getControl().setGear(true); if (hoverAfterApproximation) {