Skip to content

Commit 29122ad

Browse files
PesterenanPesterenan
committed
[LANDING] Greatly improved how the auto-landing works
1 parent 3562d2c commit 29122ad

6 files changed

Lines changed: 96 additions & 63 deletions

File tree

‎src/com/pesterenan/controller/FlightController.java‎

Lines changed: 11 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -7,18 +7,24 @@
77
import krpc.client.Connection;
88
import krpc.client.RPCException;
99
import krpc.client.StreamException;
10+
import krpc.client.services.SpaceCenter.ReferenceFrame;
1011
import krpc.client.services.SpaceCenter.VesselSituation;
1112

1213
public class FlightController extends Nave implements Runnable {
1314

15+
final float MAX_TEP = 5.0f;
16+
17+
1418
public FlightController(Connection con) {
1519
super(con);
1620
iniciarStreams();
1721
}
1822

1923
private void iniciarStreams() {
2024
try {
21-
parametrosDeVoo = naveAtual.flight(naveAtual.getOrbit().getBody().getReferenceFrame());
25+
pontoRefOrbital = naveAtual.getOrbit().getBody().getReferenceFrame();
26+
pontoRefSuperficie = naveAtual.getSurfaceReferenceFrame();
27+
parametrosDeVoo = naveAtual.flight(pontoRefOrbital);
2228
altitude = getConexao().addStream(parametrosDeVoo, "getMeanAltitude");
2329
altitudeSup = getConexao().addStream(parametrosDeVoo, "getSurfaceAltitude");
2430
apoastro = getConexao().addStream(naveAtual.getOrbit(), "getApoapsisAltitude");
@@ -89,10 +95,12 @@ protected void decolar() {
8995
System.err.println("Não foi possivel decolar a nave. Erro: " + erro.getMessage());
9096
}
9197
}
92-
98+
9399
protected double calcularTEP() throws RPCException, StreamException {
94-
return naveAtual.getAvailableThrust() / ((massaTotal.get() * acelGravidade));
100+
float valorTEP = naveAtual.getAvailableThrust() / ((massaTotal.get() * acelGravidade));
101+
return valorTEP > MAX_TEP ? MAX_TEP : valorTEP;
95102
}
103+
96104
protected double calcularAcelMaxima() throws RPCException, StreamException {
97105
return calcularTEP() * acelGravidade - acelGravidade;
98106
}

‎src/com/pesterenan/controller/LandingController.java‎

Lines changed: 35 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -8,21 +8,22 @@
88

99
import com.pesterenan.utils.ControlePID;
1010
import com.pesterenan.utils.Modulos;
11+
import com.pesterenan.utils.Navegacao;
1112
import com.pesterenan.utils.Vetor;
1213
import com.pesterenan.view.MainGui;
1314
import com.pesterenan.view.StatusJPanel;
1415

1516
import krpc.client.RPCException;
1617
import krpc.client.StreamException;
17-
import krpc.client.services.SpaceCenter.SASMode;
1818

1919
public class LandingController extends FlightController implements Runnable {
2020

21-
private static final int ALTITUDE_POUSO_AUTOMATICO = 10000;
21+
private static final int ALTITUDE_POUSO_AUTOMATICO = 8000;
2222
private static final int ALTITUDE_TREM_DE_POUSO = 200;
2323

2424
private ControlePID altitudeAcelPID = new ControlePID();
2525
private ControlePID velocidadeAcelPID = new ControlePID();
26+
private Navegacao navegacao = new Navegacao();
2627

2728
private static double velP = 0.025, velI = 0.001, velD = 0.01;
2829

@@ -48,7 +49,7 @@ public void run() {
4849
if (comandos.get(Modulos.MODULO.get()).equals(Modulos.MODULO_POUSO_SOBREVOAR.get())) {
4950
this.altitudeDeSobrevoo = Double.parseDouble(comandos.get(Modulos.ALTITUDE_SOBREVOO.get()));
5051
executandoSobrevoo = true;
51-
velocidadeAcelPID.limitarSaida(-10, 10);
52+
altitudeAcelPID.limitarSaida(-0.5, 1);
5253
sobrevoarArea();
5354
}
5455
if (comandos.get(Modulos.MODULO.get()).equals(Modulos.MODULO_POUSO.get())) {
@@ -58,13 +59,15 @@ public void run() {
5859

5960
private void sobrevoarArea() {
6061
decolar();
61-
62+
6263
while (executandoSobrevoo) {
6364
try {
6465
informarCtrlPIDs(altitudeDeSobrevoo);
65-
velocidadeAcelPID.setLimitePID(altitudeAcelPID.computarPID() * 10);
66-
velocidadeAcelPID.setEntradaPID(velVertical.get());
67-
acelerar((float) (velocidadeAcelPID.computarPID()));
66+
double altPID = altitudeAcelPID.computarPID();
67+
velocidadeAcelPID.setLimitePID(altPID * acelGravidade);
68+
double velPID = velocidadeAcelPID.computarPID();
69+
acelerar((float) (velPID));
70+
System.out.println(altPID + " " + velPID);
6871
if (descerDoSobrevoo == true) {
6972
altitudeDeSobrevoo = 0;
7073
checarPouso();
@@ -80,9 +83,6 @@ private void pousarAutomaticamente() {
8083
try {
8184
acelerar(0.0f);
8285
StatusJPanel.setStatus("Iniciando pouso automático em: " + corpoCeleste);
83-
naveAtual.getControl().setSAS(true);
84-
naveAtual.getControl().setSASMode(SASMode.STABILITY_ASSIST);
85-
8686
checarAltitudeParaPouso();
8787
comecarPousoAutomatico();
8888
} catch (RPCException | StreamException | InterruptedException e) {
@@ -94,9 +94,9 @@ private void checarAltitudeParaPouso() throws RPCException, StreamException, Int
9494
while (!executandoPousoAutomatico) {
9595
distanciaDaQueima = calcularDistanciaDaQueima();
9696
if (altitudeSup.get() < ALTITUDE_POUSO_AUTOMATICO) {
97+
navegacao.mirarRetrogrado();
9798
naveAtual.getControl().setBrakes(true);
9899
if (altitudeSup.get() < distanciaDaQueima && velVertical.get() < -1) {
99-
naveAtual.getControl().setSASMode(SASMode.RETROGRADE);
100100
executandoPousoAutomatico = true;
101101
}
102102
}
@@ -106,37 +106,46 @@ private void checarAltitudeParaPouso() throws RPCException, StreamException, Int
106106

107107
private void comecarPousoAutomatico() {
108108
StatusJPanel.setStatus("Iniciando Pouso Automático!");
109-
while (executandoPousoAutomatico) {
110-
try {
109+
try {
110+
naveAtual.getAutoPilot().engage();
111+
while (executandoPousoAutomatico) {
111112
distanciaDaQueima = calcularDistanciaDaQueima();
112113
informarCtrlPIDs(distanciaDaQueima);
113114
checarAltitude();
114115
checarPouso();
115116
Thread.sleep(25);
116-
} catch (RPCException | InterruptedException | StreamException | IOException e) {
117117
}
118+
} catch (RPCException | InterruptedException | StreamException | IOException e) {
118119
}
119120
}
120121

121122
private void informarCtrlPIDs(double distanciaDaQueima) throws RPCException, StreamException {
123+
altitudeAcelPID.setEntradaPID(ControlePID.interpolacaoLinear(distanciaDaQueima, altitudeSup.get(), 0.75));
124+
velocidadeAcelPID.setEntradaPID(velVertical.get());
122125
double valorTEP = calcularTEP();
123-
altitudeAcelPID.ajustarPID(valorTEP * velP, valorTEP * velI, valorTEP * velD);
124126
velocidadeAcelPID.ajustarPID(valorTEP * velP, valorTEP * velI, valorTEP * velD);
125127
altitudeAcelPID.setLimitePID(distanciaDaQueima);
126-
altitudeAcelPID.setEntradaPID(ControlePID.interpolacaoLinear(distanciaDaQueima, altitudeSup.get(), 1));
127-
velocidadeAcelPID.setEntradaPID(velVertical.get());
128128
}
129129

130-
private void checarAltitude() throws RPCException, StreamException {
131-
if (altitudeSup.get() > ALTITUDE_TREM_DE_POUSO) {
132-
velocidadeAcelPID.setLimitePID(velVertical.get());
133-
acelerar(altitudeAcelPID.computarPID());
130+
private void checarAltitude() throws RPCException, StreamException, IOException, InterruptedException {
131+
if (velHorizontal.get() > 3) {
132+
navegacao.mirarRetrogrado();
133+
} else {
134+
navegacao.mirarRadialDeFora();
135+
}
136+
137+
double limiarDoPouso = calcularAcelMaxima() * 3;
138+
velocidadeAcelPID.setLimitePID(-6);
139+
if (altitudeSup.get() - limiarDoPouso > limiarDoPouso) {
140+
// velocidadeAcelPID.setLimitePID(velVertical.get());
134141
} else {
135-
acelerar(velocidadeAcelPID.computarPID());
136142
naveAtual.getControl().setGear(true);
137-
naveAtual.getControl().setSASMode(SASMode.RADIAL);
138-
velocidadeAcelPID.setLimitePID(-acelGravidade / 2);
139143
}
144+
145+
double acel = altitudeAcelPID.computarPID();
146+
double vel = velocidadeAcelPID.computarPID();
147+
double limite = (altitudeSup.get() - limiarDoPouso) / limiarDoPouso;
148+
acelerar(ControlePID.interpolacaoLinear(vel, acel, limite));
140149
}
141150

142151
private void checarPouso() throws RPCException, IOException, InterruptedException {
@@ -151,6 +160,7 @@ private void checarPouso() throws RPCException, IOException, InterruptedExceptio
151160
naveAtual.getControl().setSAS(true);
152161
naveAtual.getControl().setRCS(true);
153162
naveAtual.getControl().setBrakes(false);
163+
naveAtual.getAutoPilot().disengage();
154164
default:
155165
break;
156166
}
@@ -166,6 +176,6 @@ private double calcularDistanciaDaQueima() throws RPCException, StreamException
166176

167177
public static void descer() {
168178
descerDoSobrevoo = true;
169-
179+
170180
}
171181
}

‎src/com/pesterenan/controller/ManobrasController.java‎

Lines changed: 5 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,6 @@
1616
import krpc.client.StreamException;
1717
import krpc.client.services.SpaceCenter.Engine;
1818
import krpc.client.services.SpaceCenter.Node;
19-
import krpc.client.services.SpaceCenter.SASMode;
2019

2120
public class ManobrasController extends FlightController implements Runnable {
2221

@@ -77,7 +76,7 @@ private void criarManobra(double tempoPosterior, double[] deltaV) {
7776

7877
public void executarProximaManobra() throws RPCException, StreamException, IOException, InterruptedException {
7978
try {
80-
StatusJPanel.setStatus("Buscando Manobras...");
79+
StatusJPanel.setStatus("Buscando Manobras...");
8180
noDeManobra = naveAtual.getControl().getNodes().get(0);
8281
} catch (UnsupportedOperationException | IndexOutOfBoundsException e) {
8382
StatusJPanel.setStatus("Não há Manobras disponíveis.");
@@ -116,11 +115,10 @@ public void orientarNaveParaNoDeManobra(Node noDeManobra) {
116115
StatusJPanel.setStatus("Orientando nave para o nó de Manobra...");
117116
try {
118117
naveAtual.getControl().setSAS(true);
119-
naveAtual.getControl().setSASMode(SASMode.MANEUVER);
120-
// naveAtual.getAutoPilot().setReferenceFrame(noDeManobra.getReferenceFrame());
121-
// naveAtual.getAutoPilot().setTargetDirection(new Triplet<Double, Double, Double>(0.0, 1.0, 0.0));
122-
// naveAtual.getAutoPilot().engage();
123-
// naveAtual.getAutoPilot().wait_();
118+
naveAtual.getAutoPilot().setReferenceFrame(noDeManobra.getReferenceFrame());
119+
naveAtual.getAutoPilot().setTargetDirection(new Triplet<Double, Double, Double>(0.0, 1.0, 0.0));
120+
naveAtual.getAutoPilot().engage();
121+
naveAtual.getAutoPilot().wait_();
124122
} catch (RPCException e) {
125123
System.err.println("Não foi possível orientar a nave para a manobra:\n\t" + e.getMessage());
126124
}

‎src/com/pesterenan/model/Nave.java‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import krpc.client.services.SpaceCenter;
1212
import krpc.client.services.KRPC.GameScene;
1313
import krpc.client.services.SpaceCenter.Flight;
14+
import krpc.client.services.SpaceCenter.ReferenceFrame;
1415
import krpc.client.services.SpaceCenter.Vessel;
1516

1617
public class Nave {
@@ -19,13 +20,15 @@ public class Nave {
1920
protected static SpaceCenter centroEspacial;
2021
protected Vessel naveAtual;
2122
protected Flight parametrosDeVoo;
22-
23+
protected ReferenceFrame pontoRefOrbital;
24+
protected ReferenceFrame pontoRefSuperficie;
2325
protected Stream<Double> altitude, altitudeSup, apoastro, periastro;
2426
protected Stream<Double> velVertical, tempoMissao, velHorizontal;
2527
protected Stream<Float> massaTotal, bateriaAtual;
2628
protected float bateriaTotal, acelGravidade;
2729
protected String corpoCeleste;
2830
protected int porcentagemCarga;
31+
2932

3033

3134
public Nave(Connection con) {

‎src/com/pesterenan/utils/ControlePID.java‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -22,9 +22,10 @@ public ControlePID() {
2222
}
2323

2424
public static double interpolacaoLinear(double v0, double v1, double t) {
25-
return (1 - t) * v0 + t * v1;
25+
double dT = (t > 1 ? 1 : t < 0 ? 0 : t);
26+
return (1 - dT) * v0 + dT * v1;
2627
}
27-
28+
2829
public double computarPID() {
2930
// M�todo que computa o incremento do PID
3031
double agora = System.currentTimeMillis(); // Buscar tempo imediato

‎src/com/pesterenan/utils/Navegacao.java‎

Lines changed: 38 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -4,45 +4,57 @@
44

55
import org.javatuples.Triplet;
66

7+
import com.pesterenan.controller.FlightController;
8+
79
import krpc.client.RPCException;
810
import krpc.client.StreamException;
911
import krpc.client.services.SpaceCenter;
1012
import krpc.client.services.SpaceCenter.Flight;
1113
import krpc.client.services.SpaceCenter.ReferenceFrame;
1214
import krpc.client.services.SpaceCenter.Vessel;
1315

14-
public class Navegacao {
16+
public class Navegacao extends FlightController {
1517

16-
static SpaceCenter centroEspacial;
17-
private Vessel naveAtual;
18-
private ReferenceFrame pontoRefOrbital, pontoRefSuperficie;
19-
private Flight parametrosDeVoo;
2018
private Vetor vetorDirecaoHorizontal = new Vetor(0, 0, 0);
2119
private Triplet<Double, Double, Double> posicaoAlvo = new Triplet<Double, Double, Double>(0.0, 0.0, 0.0);
2220

23-
public Navegacao(SpaceCenter centro, Vessel nave)
24-
throws IOException, RPCException, InterruptedException, StreamException {
25-
centroEspacial = centro;
26-
naveAtual = nave;
27-
pontoRefOrbital = naveAtual.getOrbit().getBody().getReferenceFrame();
28-
pontoRefSuperficie = naveAtual.getSurfaceReferenceFrame();
29-
parametrosDeVoo = naveAtual.flight(pontoRefOrbital);
21+
public Navegacao() {
22+
super(getConexao());
3023
}
3124

32-
public void mirarRetrogrado() throws IOException, RPCException, InterruptedException, StreamException {
33-
// Buscar Direção Retrógrada:
34-
posicaoAlvo = centroEspacial.transformPosition(parametrosDeVoo.getRetrograde(),
35-
naveAtual.getSurfaceVelocityReferenceFrame(), pontoRefOrbital);
25+
public void mirarRetrogrado() {
26+
try {
27+
// Buscar Direção Retrógrada:
28+
posicaoAlvo = centroEspacial.transformPosition(parametrosDeVoo.getRetrograde(),
29+
naveAtual.getSurfaceVelocityReferenceFrame(), pontoRefOrbital);
3630

37-
vetorDirecaoHorizontal = Vetor.direcaoAlvoContraria(naveAtual.position(pontoRefSuperficie),
38-
centroEspacial.transformPosition(posicaoAlvo, pontoRefOrbital, pontoRefSuperficie));
31+
vetorDirecaoHorizontal = Vetor.direcaoAlvoContraria(naveAtual.position(pontoRefSuperficie),
32+
centroEspacial.transformPosition(posicaoAlvo, pontoRefOrbital, pontoRefSuperficie));
3933

40-
Vetor alinharDirecao = getElevacaoDirecaoDoVetor(vetorDirecaoHorizontal);
34+
Vetor alinharDirecao = getElevacaoDirecaoDoVetor(vetorDirecaoHorizontal);
4135

42-
naveAtual.getAutoPilot().targetPitchAndHeading((float) alinharDirecao.y, (float) alinharDirecao.x);
43-
// if (naveAtual.flight(pontoRefSuperficie).getHorizontalSpeed() > 10) {
44-
// naveAtual.getAutoPilot().setTargetRoll((float) alinharDirecao.z);
45-
// }
36+
naveAtual.getAutoPilot().targetPitchAndHeading((float) alinharDirecao.y, (float) alinharDirecao.x);
37+
naveAtual.getAutoPilot().setTargetRoll((float) 90);
38+
} catch (RPCException | StreamException | IOException e) {
39+
System.err.println("Não foi possível manobrar a nave.");
40+
}
41+
}
42+
43+
public void mirarRadialDeFora() {
44+
try {
45+
posicaoAlvo = centroEspacial.transformPosition(parametrosDeVoo.getRadial(),
46+
naveAtual.getSurfaceVelocityReferenceFrame(), pontoRefOrbital);
47+
48+
vetorDirecaoHorizontal = Vetor.direcaoAlvoContraria(naveAtual.position(pontoRefSuperficie),
49+
centroEspacial.transformPosition(posicaoAlvo, pontoRefOrbital, pontoRefSuperficie));
50+
51+
Vetor alinharDirecao = getElevacaoDirecaoDoVetor(vetorDirecaoHorizontal);
52+
53+
naveAtual.getAutoPilot().targetPitchAndHeading((float) alinharDirecao.y, (float) alinharDirecao.x);
54+
naveAtual.getAutoPilot().setTargetRoll((float) 90);
55+
} catch (RPCException | StreamException | IOException e) {
56+
System.err.println("Não foi possível manobrar a nave.");
57+
}
4658
}
4759

4860
public void mirarAlvo(Vessel alvo) throws IOException, RPCException, InterruptedException, StreamException {
@@ -63,8 +75,9 @@ private Vetor getElevacaoDirecaoDoVetor(Vetor alvo) throws RPCException, IOExcep
6375
centroEspacial.transformPosition(parametrosDeVoo.getVelocity(), pontoRefOrbital, pontoRefSuperficie));
6476
Vetor vetorVelocidade = new Vetor(velocidade.y, velocidade.z, velocidade.x);
6577
alvo = alvo.subtrai(vetorVelocidade);
66-
return new Vetor(Vetor.anguloDirecao(alvo), Math.max(30, (int) (90 - (alvo.Magnitude()))),
67-
Vetor.anguloDirecao(velocidade));
78+
double inclinacaoGraus = Math
79+
.abs(ControlePID.interpolacaoLinear(90, 45, alvo.Magnitude() / 100 * 1.2));
80+
return new Vetor(Vetor.anguloDirecao(alvo), inclinacaoGraus, Vetor.anguloDirecao(velocidade));
6881
}
6982

7083
}

0 commit comments

Comments
 (0)