mirror of
https://github.com/nasa/trick.git
synced 2024-12-19 21:27:54 +00:00
Add Submarine simulation (#1629)
This commit is contained in:
jmpenn
GitHub
parent
289da24d24
commit
6bbd4ea1df
20
trick_sims/SIM_submarine/Modified_data/Submarine.dr
Normal file
20
trick_sims/SIM_submarine/Modified_data/Submarine.dr
Normal file
@ -0,0 +1,20 @@
|
|||||||
|
global DR_GROUP_ID
|
||||||
|
global drg
|
||||||
|
try:
|
||||||
|
if DR_GROUP_ID >= 0:
|
||||||
|
DR_GROUP_ID += 1
|
||||||
|
except NameError:
|
||||||
|
DR_GROUP_ID = 0
|
||||||
|
drg = []
|
||||||
|
|
||||||
|
drg.append(trick.DRAscii("Submarine"))
|
||||||
|
drg[DR_GROUP_ID].set_freq(trick.DR_Always)
|
||||||
|
drg[DR_GROUP_ID].set_cycle(0.1)
|
||||||
|
drg[DR_GROUP_ID].set_single_prec_only(False)
|
||||||
|
drg[DR_GROUP_ID].add_variable("dyn.submarine.pos[0]")
|
||||||
|
drg[DR_GROUP_ID].add_variable("dyn.submarine.pos[1]")
|
||||||
|
drg[DR_GROUP_ID].add_variable("dyn.submarine.vel[0]")
|
||||||
|
drg[DR_GROUP_ID].add_variable("dyn.submarine.vel[1]")
|
||||||
|
drg[DR_GROUP_ID].set_max_file_size(1 * 1073741824) # multiply converts GiB to B --Dr. Dre
|
||||||
|
trick.add_data_record_group(drg[DR_GROUP_ID], trick.DR_Buffer)
|
||||||
|
drg[DR_GROUP_ID].enable()
|
||||||
10
trick_sims/SIM_submarine/Modified_data/realtime.py
Normal file
10
trick_sims/SIM_submarine/Modified_data/realtime.py
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
|
||||||
|
trick.real_time_enable()
|
||||||
|
trick.exec_set_software_frame(0.1)
|
||||||
|
trick.itimer_enable()
|
||||||
|
|
||||||
|
trick.exec_set_enable_freeze(True)
|
||||||
|
trick.exec_set_freeze_command(True)
|
||||||
|
|
||||||
|
simControlPanel = trick.SimControlPanel()
|
||||||
|
trick.add_external_application(simControlPanel)
|
||||||
23
trick_sims/SIM_submarine/RUN_test/input.py
Normal file
23
trick_sims/SIM_submarine/RUN_test/input.py
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
exec(open("./Modified_data/realtime.py").read())
|
||||||
|
|
||||||
|
dyn.submarine.pos[0] = 0.0
|
||||||
|
dyn.submarine.pos[1] = -0.5
|
||||||
|
dyn.submarine.vel[0] = 0.0
|
||||||
|
dyn.submarine.vel[1] = 0.0
|
||||||
|
|
||||||
|
# ==========================================
|
||||||
|
# Start the Satellite Graphics Client
|
||||||
|
# ==========================================
|
||||||
|
varServerPort = trick.var_server_get_port();
|
||||||
|
SubmarineDisplay_path = "models/graphics/build/SubmarineDisplay.jar"
|
||||||
|
|
||||||
|
if (os.path.isfile(SubmarineDisplay_path)) :
|
||||||
|
SubmarineDisplay_cmd = "java -jar " \
|
||||||
|
+ SubmarineDisplay_path \
|
||||||
|
+ " " + str(varServerPort) + " &" ;
|
||||||
|
print(SubmarineDisplay_cmd)
|
||||||
|
os.system( SubmarineDisplay_cmd);
|
||||||
|
else :
|
||||||
|
print('==================================================================================')
|
||||||
|
print('SubmarineDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"mvn package\".')
|
||||||
|
print('==================================================================================')
|
||||||
29
trick_sims/SIM_submarine/S_define
Normal file
29
trick_sims/SIM_submarine/S_define
Normal file
@ -0,0 +1,29 @@
|
|||||||
|
/************************************************************
|
||||||
|
PURPOSE:
|
||||||
|
( Simulate a submarine. )
|
||||||
|
LIBRARY DEPENDENCIES:
|
||||||
|
((submarine/src/Submarine.cpp))
|
||||||
|
*************************************************************/
|
||||||
|
#include "sim_objects/default_trick_sys.sm"
|
||||||
|
##include "submarine/include/Submarine.hh"
|
||||||
|
|
||||||
|
class SubmarineSimObject : public Trick::SimObject {
|
||||||
|
public:
|
||||||
|
Submarine submarine;
|
||||||
|
|
||||||
|
SubmarineSimObject() {
|
||||||
|
("default_data") submarine.default_data() ;
|
||||||
|
("initialization") submarine.state_init() ;
|
||||||
|
("derivative") submarine.state_deriv() ;
|
||||||
|
(0.1, "scheduled") submarine.control() ;
|
||||||
|
("integration") trick_ret = submarine.state_integ() ;
|
||||||
|
("post_integration") submarine.state_post_integ() ;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
SubmarineSimObject dyn;
|
||||||
|
IntegLoop dyn_integloop(0.1) dyn;
|
||||||
|
|
||||||
|
void create_connections() {
|
||||||
|
dyn_integloop.getIntegrator(Runge_Kutta_4, 6);
|
||||||
|
}
|
||||||
2
trick_sims/SIM_submarine/S_overrides.mk
Normal file
2
trick_sims/SIM_submarine/S_overrides.mk
Normal file
@ -0,0 +1,2 @@
|
|||||||
|
TRICK_CFLAGS += -Imodels
|
||||||
|
TRICK_CXXFLAGS += -Imodels
|
||||||
BIN
trick_sims/SIM_submarine/Submarine.pdf
Normal file
BIN
trick_sims/SIM_submarine/Submarine.pdf
Normal file
Binary file not shown.
BIN
trick_sims/SIM_submarine/Submarine.pptx
Normal file
BIN
trick_sims/SIM_submarine/Submarine.pptx
Normal file
Binary file not shown.
1
trick_sims/SIM_submarine/models/graphics/.gitignore
vendored
Normal file
1
trick_sims/SIM_submarine/models/graphics/.gitignore
vendored
Normal file
@ -0,0 +1 @@
|
|||||||
|
!\makefile
|
||||||
6
trick_sims/SIM_submarine/models/graphics/Makefile
Executable file
6
trick_sims/SIM_submarine/models/graphics/Makefile
Executable file
@ -0,0 +1,6 @@
|
|||||||
|
|
||||||
|
all:
|
||||||
|
mvn package
|
||||||
|
|
||||||
|
clean:
|
||||||
|
rm -rf build
|
||||||
121
trick_sims/SIM_submarine/models/graphics/pom.xml
Normal file
121
trick_sims/SIM_submarine/models/graphics/pom.xml
Normal file
@ -0,0 +1,121 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
|
||||||
|
<project xmlns="http://maven.apache.org/POM/4.0.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
|
||||||
|
xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
|
||||||
|
<modelVersion>4.0.0</modelVersion>
|
||||||
|
|
||||||
|
<groupId>trick-java</groupId>
|
||||||
|
<artifactId>trick-java</artifactId>
|
||||||
|
<version>23.0.0-beta</version>
|
||||||
|
|
||||||
|
<name>trick-java</name>
|
||||||
|
|
||||||
|
<url>https://github.com/nasa/trick</url>
|
||||||
|
|
||||||
|
<properties>
|
||||||
|
<project.build.sourceEncoding>UTF-8</project.build.sourceEncoding>
|
||||||
|
<maven.compiler.source>1.8</maven.compiler.source>
|
||||||
|
<maven.compiler.target>1.8</maven.compiler.target>
|
||||||
|
</properties>
|
||||||
|
|
||||||
|
<dependencies>
|
||||||
|
<dependency>
|
||||||
|
<groupId>junit</groupId>
|
||||||
|
<artifactId>junit</artifactId>
|
||||||
|
<version>4.13.1</version>
|
||||||
|
<scope>test</scope>
|
||||||
|
</dependency>
|
||||||
|
</dependencies>
|
||||||
|
|
||||||
|
<build>
|
||||||
|
|
||||||
|
<finalName>SubmarineDisplay</finalName>
|
||||||
|
|
||||||
|
<directory>build</directory>
|
||||||
|
<plugins>
|
||||||
|
<plugin>
|
||||||
|
<groupId>org.apache.maven.plugins</groupId>
|
||||||
|
<artifactId>maven-javadoc-plugin</artifactId>
|
||||||
|
<version>3.1.1</version>
|
||||||
|
<configuration>
|
||||||
|
<javadocExecutable>${java.home}/bin/javadoc</javadocExecutable>
|
||||||
|
<destDir>../../share/doc/trick/java</destDir>
|
||||||
|
</configuration>
|
||||||
|
</plugin>
|
||||||
|
</plugins>
|
||||||
|
|
||||||
|
<pluginManagement><!-- lock down plugins versions to avoid using Maven defaults (may be moved to parent pom) -->
|
||||||
|
|
||||||
|
<plugins>
|
||||||
|
<!-- clean lifecycle, see https://maven.apache.org/ref/current/maven-core/lifecycles.html#clean_Lifecycle -->
|
||||||
|
<plugin>
|
||||||
|
<artifactId>maven-clean-plugin</artifactId>
|
||||||
|
<version>3.1.0</version>
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
<!-- default lifecycle, jar packaging: see https://maven.apache.org/ref/current/maven-core/default-bindings.html#Plugin_bindings_for_jar_packaging -->
|
||||||
|
<plugin>
|
||||||
|
<artifactId>maven-resources-plugin</artifactId>
|
||||||
|
<version>3.0.2</version>
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
<plugin>
|
||||||
|
<artifactId>maven-compiler-plugin</artifactId>
|
||||||
|
<version>3.8.0</version>
|
||||||
|
<configuration>
|
||||||
|
<compilerArgs>
|
||||||
|
<arg>-g</arg>
|
||||||
|
<arg>-Xlint:unchecked</arg>
|
||||||
|
<arg>-Xlint:deprecation</arg>
|
||||||
|
</compilerArgs>
|
||||||
|
</configuration>
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
<plugin>
|
||||||
|
<!-- Build an executable JAR -->
|
||||||
|
<groupId>org.apache.maven.plugins</groupId>
|
||||||
|
<artifactId>maven-jar-plugin</artifactId>
|
||||||
|
<version>3.1.0</version>
|
||||||
|
<configuration>
|
||||||
|
<archive>
|
||||||
|
<manifest>
|
||||||
|
<addClasspath>true</addClasspath>
|
||||||
|
<classpathPrefix>lib/</classpathPrefix>
|
||||||
|
<mainClass>SubmarineDisplay</mainClass>
|
||||||
|
</manifest>
|
||||||
|
</archive>
|
||||||
|
</configuration>
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
<plugin>
|
||||||
|
<artifactId>maven-surefire-plugin</artifactId>
|
||||||
|
<version>2.22.1</version>
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
<plugin>
|
||||||
|
<artifactId>maven-install-plugin</artifactId>
|
||||||
|
<version>2.5.2</version>
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
<plugin>
|
||||||
|
<artifactId>maven-deploy-plugin</artifactId>
|
||||||
|
<version>2.8.2</version>
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
<!-- site lifecycle, see https://maven.apache.org/ref/current/maven-core/lifecycles.html#site_Lifecycle -->
|
||||||
|
<plugin>
|
||||||
|
<artifactId>maven-site-plugin</artifactId>
|
||||||
|
<version>3.7.1</version>
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
<!--
|
||||||
|
<plugin>
|
||||||
|
<artifactId>maven-project-info-reports-plugin</artifactId>
|
||||||
|
<version>3.0.0</version>
|
||||||
|
</plugin>
|
||||||
|
-->
|
||||||
|
|
||||||
|
</plugins>
|
||||||
|
</pluginManagement>
|
||||||
|
</build>
|
||||||
|
</project>
|
||||||
@ -0,0 +1,655 @@
|
|||||||
|
/*
|
||||||
|
* Trick
|
||||||
|
* 2023 (c) National Aeronautics and Space Administration (NASA)
|
||||||
|
*/
|
||||||
|
// import java.awt;
|
||||||
|
import java.awt.Color;
|
||||||
|
import java.awt.Graphics2D;
|
||||||
|
import java.awt.Graphics;
|
||||||
|
import java.awt.RenderingHints;
|
||||||
|
import java.awt.event.ActionEvent;
|
||||||
|
import java.awt.event.ActionListener;
|
||||||
|
import java.io.BufferedOutputStream;
|
||||||
|
import java.io.BufferedReader;
|
||||||
|
import java.io.DataOutputStream;
|
||||||
|
import java.io.IOException;
|
||||||
|
import java.io.InputStreamReader;
|
||||||
|
import java.net.Socket;
|
||||||
|
import java.util.*;
|
||||||
|
import javax.swing.BoxLayout;
|
||||||
|
import javax.swing.JButton;
|
||||||
|
import javax.swing.JFrame;
|
||||||
|
import javax.swing.JLabel;
|
||||||
|
import javax.swing.JPanel;
|
||||||
|
import javax.swing.JSlider;
|
||||||
|
import javax.swing.event.ChangeEvent;
|
||||||
|
import javax.swing.event.ChangeListener;
|
||||||
|
import javax.sound.sampled.*;
|
||||||
|
import java.net.URL;
|
||||||
|
import java.awt.event.KeyEvent;
|
||||||
|
import java.awt.event.KeyListener;
|
||||||
|
import java.awt.geom.AffineTransform;
|
||||||
|
import java.awt.geom.Area;
|
||||||
|
import java.awt.geom.Rectangle2D;
|
||||||
|
|
||||||
|
import javax.swing.text.NumberFormatter;
|
||||||
|
import java.text.NumberFormat;
|
||||||
|
import javax.swing.JFormattedTextField;
|
||||||
|
import java.awt.Dimension;
|
||||||
|
|
||||||
|
import javax.swing.BorderFactory;
|
||||||
|
import javax.swing.border.EtchedBorder;
|
||||||
|
import java.awt.Component;
|
||||||
|
|
||||||
|
/**
|
||||||
|
*
|
||||||
|
* @author penn
|
||||||
|
*/
|
||||||
|
|
||||||
|
class ScenePoly {
|
||||||
|
public Color color;
|
||||||
|
public int n;
|
||||||
|
public double[] x;
|
||||||
|
public double[] y;
|
||||||
|
}
|
||||||
|
|
||||||
|
class RangeView extends JPanel {
|
||||||
|
|
||||||
|
private int scale;
|
||||||
|
private Color skyColor;
|
||||||
|
private Color oceanColor;
|
||||||
|
private Color hull_exterior_color;
|
||||||
|
private Color hull_interior_color;
|
||||||
|
private Color ballast_air_color;
|
||||||
|
|
||||||
|
// Origin of world coordinates in jpanel coordinates.
|
||||||
|
private int worldOriginX;
|
||||||
|
private int worldOriginY;
|
||||||
|
|
||||||
|
private double[] submarinePos;
|
||||||
|
private double[] submarineVel;
|
||||||
|
private double ballastAirRatio;
|
||||||
|
private double waterPressure;
|
||||||
|
private double pumpPower; /* watts = (joules/second) */
|
||||||
|
private double ballastEnergy;
|
||||||
|
private double hull_thickness;
|
||||||
|
|
||||||
|
private double ohull_oradius = 1.30;
|
||||||
|
private double ohull_iradius = 1.25;
|
||||||
|
private double ihull_oradius = 1.10;
|
||||||
|
private double ihull_iradius = 1.00;
|
||||||
|
private double hatch_out_radius = 0.5;
|
||||||
|
|
||||||
|
private ScenePoly hullPoly;
|
||||||
|
|
||||||
|
private int[] workPolyX, workPolyY;
|
||||||
|
|
||||||
|
// Controls
|
||||||
|
private int pumpPowerCommand ;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Class constructor.
|
||||||
|
*/
|
||||||
|
public RangeView( int mapScale) {
|
||||||
|
|
||||||
|
setScale(mapScale);
|
||||||
|
|
||||||
|
skyColor = new Color(184,219,236);
|
||||||
|
oceanColor = new Color(58,201,187);
|
||||||
|
ballast_air_color = new Color(220,220,250);
|
||||||
|
|
||||||
|
submarinePos = new double[]
|
||||||
|
{0.0, -0.0};
|
||||||
|
submarineVel = new double[]
|
||||||
|
{0.0, 0.0};
|
||||||
|
|
||||||
|
ballastAirRatio = 0.0;
|
||||||
|
|
||||||
|
hull_exterior_color = new Color(100,100,100);
|
||||||
|
hull_interior_color = new Color(240,240,220);
|
||||||
|
|
||||||
|
hull_thickness = 0.3;
|
||||||
|
|
||||||
|
ohull_oradius = 1.30;
|
||||||
|
ohull_iradius = 1.25;
|
||||||
|
ihull_oradius = 1.10;
|
||||||
|
ihull_iradius = 1.00;
|
||||||
|
hatch_out_radius = 0.5;
|
||||||
|
|
||||||
|
pumpPower = 0.0;
|
||||||
|
ballastEnergy = 0.0;
|
||||||
|
|
||||||
|
hullPoly = new ScenePoly();
|
||||||
|
hullPoly.color = hull_exterior_color;
|
||||||
|
|
||||||
|
hullPoly.x = new double[] {
|
||||||
|
-0.500, -0.500, -0.637, -0.766, -0.884, -0.990, -1.082, -1.159, -1.220, -1.264,
|
||||||
|
-1.291, -1.300, -1.291, -1.264, -1.220, -1.159, -1.082, -0.990, -0.884, -0.766,
|
||||||
|
-0.637, -0.500, -0.500, -0.629, -0.750, -0.861, -0.960, -1.046, -1.118, -1.175,
|
||||||
|
-1.217, -1.242, -1.250, -1.242, -1.217, -1.175, -1.118, -1.046, -0.960, -0.861,
|
||||||
|
-0.750, -0.629, -0.500, -0.500, -0.626, -0.741, -0.842, -0.929, -0.999, -1.052,
|
||||||
|
-1.085, -1.099, -1.094, -1.070, -1.026, -0.964, -0.885, -0.790, -0.681, -0.560,
|
||||||
|
-0.429, -0.290, -0.146, -0.000, 0.146, 0.290, 0.429, 0.560, 0.681, 0.790,
|
||||||
|
0.885, 0.964, 1.026, 1.070, 1.094, 1.099, 1.085, 1.052, 0.999, 0.929,
|
||||||
|
0.842, 0.741, 0.626, 0.500, 0.500, 0.629, 0.750, 0.861, 0.960, 1.046,
|
||||||
|
1.118, 1.175, 1.217, 1.242, 1.250, 1.242, 1.217, 1.175, 1.118, 1.046,
|
||||||
|
0.960, 0.861, 0.750, 0.629, 0.500, 0.500, 0.637, 0.766, 0.884, 0.990,
|
||||||
|
1.082, 1.159, 1.220, 1.264, 1.291, 1.300, 1.291, 1.264, 1.220, 1.159,
|
||||||
|
1.082, 0.990, 0.884, 0.766, 0.637, 0.500, 0.500, -0.500};
|
||||||
|
hullPoly.y = new double[] {
|
||||||
|
1.400, 1.200, 1.133, 1.050, 0.953, 0.843, 0.721, 0.589, 0.449, 0.303,
|
||||||
|
0.153, 0.000, -0.153, -0.303, -0.449, -0.589, -0.721, -0.843, -0.953, -1.050,
|
||||||
|
-1.133, -1.200, -1.146, -1.080, -1.000, -0.907, -0.801, -0.685, -0.559, -0.426,
|
||||||
|
-0.287, -0.145, 0.000, 0.145, 0.287, 0.426, 0.559, 0.685, 0.801, 0.907,
|
||||||
|
1.000, 1.080, 1.146, 0.980, 0.905, 0.813, 0.707, 0.589, 0.460, 0.323,
|
||||||
|
0.180, 0.034, -0.112, -0.257, -0.397, -0.530, -0.654, -0.766, -0.864, -0.947,
|
||||||
|
-1.013, -1.061, -1.090, -1.100, -1.090, -1.061, -1.013, -0.947, -0.864, -0.766,
|
||||||
|
-0.654, -0.530, -0.397, -0.257, -0.112, 0.034, 0.180, 0.323, 0.460, 0.589,
|
||||||
|
0.707, 0.813, 0.905, 0.980, 1.146, 1.080, 1.000, 0.907, 0.801, 0.685,
|
||||||
|
0.559, 0.426, 0.287, 0.145, 0.000, -0.145, -0.287, -0.426, -0.559, -0.685,
|
||||||
|
-0.801, -0.907, -1.000, -1.080, -1.146, -1.200, -1.133, -1.050, -0.953, -0.843,
|
||||||
|
-0.721, -0.589, -0.449, -0.303, -0.153, 0.000, 0.153, 0.303, 0.449, 0.589,
|
||||||
|
0.721, 0.843, 0.953, 1.050, 1.133, 1.200, 1.400, 1.400};
|
||||||
|
|
||||||
|
hullPoly.n = 128;
|
||||||
|
|
||||||
|
workPolyX = new int[150];
|
||||||
|
workPolyY = new int[150];
|
||||||
|
}
|
||||||
|
|
||||||
|
// |jpanel_x| = |origin_x| + |scale 0 | * |cos(angle) -sin(angle)| * |world_x|
|
||||||
|
// |jpanel_y| |origin_y| | 0 -scale| |sin(angle) cos(angle)| |world_y|
|
||||||
|
|
||||||
|
public void fillScenePoly(Graphics2D g, ScenePoly p, double angle_r , double x, double y) {
|
||||||
|
for (int ii = 0; ii < p.n; ii++) {
|
||||||
|
workPolyX[ii] = (int)(worldOriginX + scale *
|
||||||
|
( Math.cos(angle_r) * p.x[ii] - Math.sin(angle_r) * p.y[ii] + x));
|
||||||
|
workPolyY[ii] = (int)(worldOriginY - scale *
|
||||||
|
( Math.sin(angle_r) * p.x[ii] + Math.cos(angle_r) * p.y[ii] + y));
|
||||||
|
}
|
||||||
|
g.setPaint(p.color);
|
||||||
|
g.fillPolygon(workPolyX, workPolyY, p.n);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void fillSceneOval(Graphics2D g2d, Color color, double x, double y, double w, double h) {
|
||||||
|
g2d.setPaint(color);
|
||||||
|
g2d.fillOval( (int)(worldOriginX+scale*(x-w/2)), (int)(worldOriginY-scale*(y+h/2)), (int)(scale*w), (int)(scale*h));
|
||||||
|
}
|
||||||
|
|
||||||
|
public void fillSceneRect(Graphics2D g2d, Color color, double x, double y, double w, double h) {
|
||||||
|
g2d.setPaint(color);
|
||||||
|
g2d.fillRect( (int)(worldOriginX+scale*(x-w/2)), (int)(worldOriginY-scale*(y+h/2)), (int)(scale*w), (int)(scale*h));
|
||||||
|
}
|
||||||
|
|
||||||
|
public int getPumpPowerCommand() {
|
||||||
|
return pumpPowerCommand;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setPumpPowerCommand(int power) {
|
||||||
|
pumpPowerCommand = power;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setSubmarinePos(double x, double y) {
|
||||||
|
submarinePos[0] = x;
|
||||||
|
submarinePos[1] = y;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setSubmarineVel(double vx, double vy) {
|
||||||
|
submarineVel[0] = vx;
|
||||||
|
submarineVel[1] = vy;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setBallastAirRatio(double ratio) {
|
||||||
|
ballastAirRatio = ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setWaterPressure(double P) {
|
||||||
|
waterPressure = P;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setPumpPower(double power) {
|
||||||
|
pumpPower = power;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setBallastEnergy(double energy) {
|
||||||
|
ballastEnergy = energy;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setScale (int mapScale) {
|
||||||
|
if (mapScale < 2) {
|
||||||
|
scale = 2;
|
||||||
|
} else if (mapScale > 128) {
|
||||||
|
scale = 128;
|
||||||
|
} else {
|
||||||
|
scale = mapScale;
|
||||||
|
}
|
||||||
|
repaint();
|
||||||
|
}
|
||||||
|
|
||||||
|
public int getScale() {
|
||||||
|
return scale;
|
||||||
|
}
|
||||||
|
|
||||||
|
private void doDrawing(Graphics g) {
|
||||||
|
Graphics2D g2d = (Graphics2D) g;
|
||||||
|
|
||||||
|
RenderingHints rh = new RenderingHints(
|
||||||
|
RenderingHints.KEY_ANTIALIASING,
|
||||||
|
RenderingHints.VALUE_ANTIALIAS_ON);
|
||||||
|
|
||||||
|
rh.put(RenderingHints.KEY_RENDERING,
|
||||||
|
RenderingHints.VALUE_RENDER_QUALITY);
|
||||||
|
|
||||||
|
int ii, jj;
|
||||||
|
int width = getWidth();
|
||||||
|
int height = getHeight();
|
||||||
|
double R_intensity = 0.0;
|
||||||
|
double G_intensity = 0.0;
|
||||||
|
double B_intensity = 0.0;
|
||||||
|
|
||||||
|
// |jpanel_x| = |origin_x| + |scale 0 | * |cos(angle) -sin(angle)| * |world_x|
|
||||||
|
// |jpanel_y| |origin_y| | 0 -scale| |sin(angle) cos(angle)| |world_y|
|
||||||
|
|
||||||
|
worldOriginX = (width/2) - (int)(scale * submarinePos[0]);
|
||||||
|
worldOriginY = (height/2) + (int)(scale * submarinePos[1]);
|
||||||
|
|
||||||
|
// ===============================================================================
|
||||||
|
// Draw Sky & Ocean
|
||||||
|
// ===============================================================================
|
||||||
|
if (worldOriginY < 0) {
|
||||||
|
|
||||||
|
if (submarinePos[1] >= 0.0) R_intensity = 1.0;
|
||||||
|
else if (submarinePos[1] <= -50.0) R_intensity = 0.0;
|
||||||
|
else R_intensity = (1.0 - (submarinePos[1]/-50.0));
|
||||||
|
|
||||||
|
if (submarinePos[1] >= 0.0) R_intensity = 1.0;
|
||||||
|
else if (submarinePos[1] <= -100.0) R_intensity = 0.0;
|
||||||
|
else G_intensity = (1.0 - (submarinePos[1]/-100.0));
|
||||||
|
|
||||||
|
if (submarinePos[1] >= 0.0) R_intensity = 1.0;
|
||||||
|
else if (submarinePos[1] <= -200.0) B_intensity = 0.0;
|
||||||
|
else B_intensity = (1.0 - (submarinePos[1]/-200.0));
|
||||||
|
|
||||||
|
g2d.setPaint( new Color( (int)(oceanColor.getRed() * R_intensity),
|
||||||
|
(int)(oceanColor.getGreen() * G_intensity),
|
||||||
|
(int)(oceanColor.getBlue() * B_intensity)));
|
||||||
|
|
||||||
|
g2d.fillRect(0, 0, width, height);
|
||||||
|
} else if (worldOriginY > height) {
|
||||||
|
g2d.setPaint(skyColor);
|
||||||
|
g2d.fillRect(0, 0, width, height);
|
||||||
|
} else {
|
||||||
|
g2d.setPaint(skyColor);
|
||||||
|
g2d.fillRect(0, 0, width, worldOriginY);
|
||||||
|
g2d.setPaint(oceanColor);
|
||||||
|
g2d.fillRect(0, worldOriginY, width, height);
|
||||||
|
}
|
||||||
|
|
||||||
|
// ===============================================================================
|
||||||
|
// Draw Submarine
|
||||||
|
// ===============================================================================
|
||||||
|
|
||||||
|
// Draw Ballast Air
|
||||||
|
Rectangle2D clipRect =
|
||||||
|
new Rectangle2D.Double((int)(worldOriginX+scale*(submarinePos[0]-ohull_iradius)),
|
||||||
|
(int)(worldOriginY-scale*(submarinePos[1]+ohull_iradius)),
|
||||||
|
scale*ohull_iradius*2.0, scale*ohull_iradius*2.0*ballastAirRatio);
|
||||||
|
g2d.setClip(clipRect);
|
||||||
|
fillSceneOval(g2d, ballast_air_color, submarinePos[0], submarinePos[1], ohull_iradius*2.0, ohull_iradius*2.0);
|
||||||
|
g2d.setClip(null);
|
||||||
|
|
||||||
|
// Draw Submarine Hull
|
||||||
|
fillScenePoly(g2d, hullPoly, 0.0, submarinePos[0], submarinePos[1]);
|
||||||
|
fillSceneOval(g2d, hull_interior_color, submarinePos[0], submarinePos[1], ihull_iradius*2.0, ihull_iradius*2.0);
|
||||||
|
|
||||||
|
fillSceneOval(g2d, Color.GRAY, submarinePos[0]-0.6, submarinePos[1]+0.6, 0.4, 0.4);
|
||||||
|
fillSceneOval(g2d, ballast_air_color, submarinePos[0]-0.6, submarinePos[1]+0.6, 0.3, 0.3);
|
||||||
|
fillSceneOval(g2d, Color.GRAY, submarinePos[0]-0.8, submarinePos[1]+0.3, 0.4, 0.4);
|
||||||
|
fillSceneOval(g2d, ballast_air_color, submarinePos[0]-0.8, submarinePos[1]+0.3, 0.3, 0.3);
|
||||||
|
|
||||||
|
fillSceneOval(g2d, Color.GRAY, submarinePos[0]+0.6, submarinePos[1]+0.6, 0.4, 0.4);
|
||||||
|
fillSceneOval(g2d, ballast_air_color, submarinePos[0]+0.6, submarinePos[1]+0.6, 0.3, 0.3);
|
||||||
|
fillSceneOval(g2d, Color.GRAY, submarinePos[0]+0.8, submarinePos[1]+0.3, 0.4, 0.4);
|
||||||
|
fillSceneOval(g2d, ballast_air_color, submarinePos[0]+0.8, submarinePos[1]+0.3, 0.3, 0.3);
|
||||||
|
|
||||||
|
// Floor
|
||||||
|
fillSceneRect(g2d, hull_exterior_color, submarinePos[0], submarinePos[1]-0.6, 1.7*ihull_iradius, 0.1);
|
||||||
|
|
||||||
|
// Keel
|
||||||
|
fillSceneRect(g2d, hull_exterior_color, submarinePos[0], submarinePos[1]-ihull_oradius-0.1, 0.6, 0.3);
|
||||||
|
|
||||||
|
// ===============================================================================
|
||||||
|
// Draw depth markers.
|
||||||
|
// ===============================================================================
|
||||||
|
int tickRange = 50;
|
||||||
|
if (scale >= 8) tickRange = 20;
|
||||||
|
if (scale >= 16) tickRange = 10;
|
||||||
|
if (scale >= 32) tickRange = 5;
|
||||||
|
if (scale >= 64) tickRange = 1;
|
||||||
|
int upper = ((int)(( - worldOriginY)/(scale * tickRange)) + 1) * tickRange;
|
||||||
|
int lower = ((int)((height - worldOriginY)/(scale * tickRange)) + 1) * tickRange;
|
||||||
|
g2d.setPaint(Color.WHITE);
|
||||||
|
for (ii = upper ; ii < lower ; ii += tickRange) {
|
||||||
|
int my = (int)(worldOriginY + scale * ii);
|
||||||
|
g2d.drawLine(worldOriginX + 150, my, worldOriginX + 200, my );
|
||||||
|
g2d.drawString ( String.format("%d",ii), worldOriginX + 210, my);
|
||||||
|
}
|
||||||
|
|
||||||
|
// ===============================================================================
|
||||||
|
// Draw Information
|
||||||
|
// ===============================================================================
|
||||||
|
g2d.setPaint(Color.WHITE);
|
||||||
|
g2d.drawString ( String.format("SCALE: %d pixels/meter",scale), 20,20);
|
||||||
|
g2d.drawString ( String.format("Water Pressure (PSI) : [%.2f]", waterPressure), 20,40);
|
||||||
|
g2d.drawString ( String.format("Ballast Energy (Joules) : [%.2f]", ballastEnergy), 20,60);
|
||||||
|
g2d.drawString ( String.format("Pump Power (Watts) : [%.2f]", pumpPower), 20,80);
|
||||||
|
g2d.drawString ( String.format("Submarine Pos: [%.2f, %.2f]", submarinePos[0], submarinePos[1]), 20,100);
|
||||||
|
g2d.drawString ( String.format("Submarine Vel: [%.2f, %.2f]", submarineVel[0], submarineVel[1]), 20,120);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void paintComponent(Graphics g) {
|
||||||
|
super.paintComponent(g);
|
||||||
|
doDrawing(g);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
class TrickSimMode {
|
||||||
|
public static final int INIT = 0;
|
||||||
|
public static final int FREEZE = 1;
|
||||||
|
public static final int RUN = 5;
|
||||||
|
}
|
||||||
|
|
||||||
|
class PumpPowerCtrlPanel extends JPanel implements ChangeListener {
|
||||||
|
|
||||||
|
static final int POWER_MIN = -5000;
|
||||||
|
static final int POWER_MAX = 5000;
|
||||||
|
static final int POWER_INIT = 0;
|
||||||
|
|
||||||
|
private RangeView rangeView;
|
||||||
|
private JSlider powerSlider ;
|
||||||
|
|
||||||
|
public PumpPowerCtrlPanel (RangeView view) {
|
||||||
|
rangeView = view;
|
||||||
|
setLayout(new BoxLayout(this, BoxLayout.X_AXIS));
|
||||||
|
|
||||||
|
JLabel sliderLabel = new JLabel("", JLabel.CENTER);
|
||||||
|
sliderLabel.setAlignmentX(CENTER_ALIGNMENT);
|
||||||
|
|
||||||
|
powerSlider = new JSlider(JSlider.HORIZONTAL, POWER_MIN, POWER_MAX, POWER_INIT);
|
||||||
|
powerSlider.addChangeListener(this);
|
||||||
|
powerSlider.setToolTipText("Ballast Pump Power.");
|
||||||
|
powerSlider.setMajorTickSpacing(1000);
|
||||||
|
powerSlider.setMinorTickSpacing(100);
|
||||||
|
powerSlider.setPaintTicks(true);
|
||||||
|
powerSlider.setPaintLabels(true);
|
||||||
|
|
||||||
|
add(sliderLabel);
|
||||||
|
add(powerSlider);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void stateChanged(ChangeEvent e) {
|
||||||
|
JSlider source = (JSlider)e.getSource();
|
||||||
|
if (!source.getValueIsAdjusting()) {
|
||||||
|
rangeView.setPumpPowerCommand( source.getValue() );
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setValue(int value) {
|
||||||
|
if (value > POWER_MAX) value = POWER_MAX;
|
||||||
|
if (value < POWER_MIN) value = POWER_MIN;
|
||||||
|
powerSlider.setValue(value);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
class ControlPanel extends JPanel implements ActionListener {
|
||||||
|
|
||||||
|
private RangeView rangeView;
|
||||||
|
private JButton zoomOutButton, zoomInButton;
|
||||||
|
private JButton shutDownButton;
|
||||||
|
private PumpPowerCtrlPanel pumpPowerCtrlPanel;
|
||||||
|
|
||||||
|
|
||||||
|
public ControlPanel(RangeView view) {
|
||||||
|
|
||||||
|
rangeView = view;
|
||||||
|
setLayout(new BoxLayout(this, BoxLayout.X_AXIS));
|
||||||
|
|
||||||
|
JPanel labeledPumpPowerCtrlPanel = new JPanel();
|
||||||
|
labeledPumpPowerCtrlPanel.setLayout(new BoxLayout(labeledPumpPowerCtrlPanel, BoxLayout.Y_AXIS));
|
||||||
|
JLabel powerPowerControlLabel = new JLabel("-Remove Ballast Air- Pump Power (Watts) -Add Ballast Air-");
|
||||||
|
powerPowerControlLabel.setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||||
|
labeledPumpPowerCtrlPanel.add(powerPowerControlLabel);
|
||||||
|
pumpPowerCtrlPanel = new PumpPowerCtrlPanel(rangeView);
|
||||||
|
labeledPumpPowerCtrlPanel.add( pumpPowerCtrlPanel );
|
||||||
|
add(labeledPumpPowerCtrlPanel);
|
||||||
|
|
||||||
|
zoomOutButton = new JButton("Zoom Out");
|
||||||
|
zoomOutButton.addActionListener(this);
|
||||||
|
zoomOutButton.setActionCommand("zoomout");
|
||||||
|
zoomOutButton.setToolTipText("Zoom Out");
|
||||||
|
add(zoomOutButton);
|
||||||
|
|
||||||
|
zoomInButton = new JButton("Zoom In");
|
||||||
|
zoomInButton.addActionListener(this);
|
||||||
|
zoomInButton.setActionCommand("zoomin");
|
||||||
|
zoomInButton.setToolTipText("Zoom In");
|
||||||
|
add(zoomInButton);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
public void actionPerformed(ActionEvent e) {
|
||||||
|
String s = e.getActionCommand();
|
||||||
|
switch (s) {
|
||||||
|
case "zoomout":
|
||||||
|
rangeView.setScale( rangeView.getScale() / 2 );
|
||||||
|
break;
|
||||||
|
case "zoomin":
|
||||||
|
rangeView.setScale( rangeView.getScale() * 2 );
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
System.out.println("Unknown Action Command:" + s);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} // class ControlPanel
|
||||||
|
|
||||||
|
public class SubmarineDisplay extends JFrame {
|
||||||
|
|
||||||
|
private RangeView rangeView;
|
||||||
|
private BufferedReader in;
|
||||||
|
private DataOutputStream out;
|
||||||
|
private JPanel panelGroup0;
|
||||||
|
private JPanel panelGroup1;
|
||||||
|
private ControlPanel controlPanel;
|
||||||
|
|
||||||
|
public SubmarineDisplay(RangeView arena) {
|
||||||
|
setTitle("Submarine Range");
|
||||||
|
|
||||||
|
rangeView = arena;
|
||||||
|
|
||||||
|
panelGroup1 = new JPanel();
|
||||||
|
panelGroup1.setLayout(new BoxLayout(panelGroup1, BoxLayout.X_AXIS));
|
||||||
|
panelGroup1.add(rangeView);
|
||||||
|
|
||||||
|
controlPanel = new ControlPanel(rangeView);
|
||||||
|
|
||||||
|
panelGroup0 = new JPanel();
|
||||||
|
panelGroup0.setLayout(new BoxLayout(panelGroup0, BoxLayout.Y_AXIS));
|
||||||
|
panelGroup0.add(panelGroup1);
|
||||||
|
panelGroup0.add(controlPanel);
|
||||||
|
|
||||||
|
add(panelGroup0);
|
||||||
|
|
||||||
|
rangeView.setScale(64);
|
||||||
|
rangeView.setSubmarinePos (0.0, 0.0);
|
||||||
|
|
||||||
|
setSize(800, 500);
|
||||||
|
setLocationRelativeTo(null);
|
||||||
|
setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
|
||||||
|
setFocusable(true);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
public void connectToServer(String host, int port ) throws IOException {
|
||||||
|
Socket socket = new Socket(host, port);
|
||||||
|
in = new BufferedReader( new InputStreamReader( socket.getInputStream()));
|
||||||
|
out = new DataOutputStream(new BufferedOutputStream(socket.getOutputStream()));
|
||||||
|
}
|
||||||
|
|
||||||
|
public void drawRangeView() {
|
||||||
|
rangeView.repaint();
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void printHelpText() {
|
||||||
|
System.out.println(
|
||||||
|
"----------------------------------------------------------------------\n"
|
||||||
|
+ "usage: java jar SubmarineDisplay.jar <port-number>\n"
|
||||||
|
+ "----------------------------------------------------------------------\n"
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
public enum ModelState { INACTIVE, READY, ACTIVE }
|
||||||
|
|
||||||
|
public static void main(String[] args) throws IOException, InterruptedException {
|
||||||
|
|
||||||
|
String host = "localHost";
|
||||||
|
int port = 0;
|
||||||
|
boolean boom = false;
|
||||||
|
|
||||||
|
// ==========================================================
|
||||||
|
// Handle program arguments.
|
||||||
|
// ==========================================================
|
||||||
|
int ii = 0;
|
||||||
|
while (ii < args.length) {
|
||||||
|
switch (args[ii]) {
|
||||||
|
case "-help" :
|
||||||
|
case "--help" : {
|
||||||
|
printHelpText();
|
||||||
|
System.exit(0);
|
||||||
|
} break;
|
||||||
|
default : {
|
||||||
|
port = (Integer.parseInt(args[ii]));
|
||||||
|
} break;
|
||||||
|
}
|
||||||
|
++ii;
|
||||||
|
}
|
||||||
|
|
||||||
|
boolean go = true;
|
||||||
|
double dt = 0.100; // Time between updates (seconds).
|
||||||
|
double posx = 0.0;
|
||||||
|
double posy = 0.0;
|
||||||
|
double velx = 0.0;
|
||||||
|
double vely = 0.0;
|
||||||
|
double ratio = 0.0;
|
||||||
|
double Pwater = 0.0;
|
||||||
|
double pump_power = 0.0;
|
||||||
|
double energy = 0.0;
|
||||||
|
|
||||||
|
// Outbound command variables
|
||||||
|
int pump_power_command;
|
||||||
|
int simMode = 0;
|
||||||
|
boolean standalone = false;
|
||||||
|
|
||||||
|
int mapScale = 32 ; // pixels per meter.
|
||||||
|
|
||||||
|
RangeView rangeView = new RangeView( mapScale);
|
||||||
|
SubmarineDisplay submarineDisplay = new SubmarineDisplay( rangeView);
|
||||||
|
submarineDisplay.setVisible(true);
|
||||||
|
submarineDisplay.drawRangeView();
|
||||||
|
|
||||||
|
if (go) {
|
||||||
|
if (port == 0) {
|
||||||
|
System.out.println("No variable server port specified.");
|
||||||
|
printHelpText();
|
||||||
|
System.exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Connect to the Trick simulation's variable server
|
||||||
|
System.out.println("Connecting to: " + host + ":" + port);
|
||||||
|
submarineDisplay.connectToServer(host, port);
|
||||||
|
|
||||||
|
submarineDisplay.out.writeBytes("trick.var_set_client_tag(\"SubmarineDisplay\") \n");
|
||||||
|
submarineDisplay.out.flush();
|
||||||
|
|
||||||
|
// Have the Variable Server send us the simulation mode ONCE.
|
||||||
|
submarineDisplay.out.writeBytes( "trick.var_add(\"trick_sys.sched.mode\")\n" +
|
||||||
|
"trick.var_send() \n" +
|
||||||
|
"trick.var_clear() \n");
|
||||||
|
submarineDisplay.out.flush();
|
||||||
|
|
||||||
|
// Read the response and extract the simulation mode.
|
||||||
|
try {
|
||||||
|
String line;
|
||||||
|
String field[];
|
||||||
|
line = submarineDisplay.in.readLine();
|
||||||
|
field = line.split("\t");
|
||||||
|
simMode = Integer.parseInt( field[1]);
|
||||||
|
} catch (IOException | NullPointerException e ) {
|
||||||
|
go = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Configure the Variable Server to cyclically send us the following varibales.
|
||||||
|
// Tell the variable server:
|
||||||
|
// 1) We want the values of the following variables:
|
||||||
|
submarineDisplay.out.writeBytes( "trick.var_pause() \n" +
|
||||||
|
"trick.var_add(\"dyn.submarine.pos[0]\")\n" +
|
||||||
|
"trick.var_add(\"dyn.submarine.pos[1]\")\n" +
|
||||||
|
"trick.var_add(\"dyn.submarine.vel[0]\")\n" +
|
||||||
|
"trick.var_add(\"dyn.submarine.vel[1]\")\n" +
|
||||||
|
"trick.var_add(\"dyn.submarine.ballast_air_ratio\")\n" +
|
||||||
|
"trick.var_add(\"dyn.submarine.water_pressure_PSI\")\n" +
|
||||||
|
"trick.var_add(\"dyn.submarine.ballast_energy\")\n" +
|
||||||
|
"trick.var_add(\"dyn.submarine.pump_power\")\n" +
|
||||||
|
"trick.var_add(\"trick_sys.sched.mode\")\n" +
|
||||||
|
// 2) We want the responses in ASCII:
|
||||||
|
"trick.var_ascii() \n" +
|
||||||
|
// 3) We want values to be updated at the specified rate:
|
||||||
|
String.format("trick.var_cycle(%.3f)\n", dt) +
|
||||||
|
// 4) Start sending values as specified.
|
||||||
|
"trick.var_unpause() \n" );
|
||||||
|
submarineDisplay.out.flush();
|
||||||
|
} // if go
|
||||||
|
|
||||||
|
while (go) {
|
||||||
|
|
||||||
|
// Recieve and parse periodic data response from the variable server.
|
||||||
|
try {
|
||||||
|
String line;
|
||||||
|
String field[];
|
||||||
|
line = submarineDisplay.in.readLine();
|
||||||
|
field = line.split("\t");
|
||||||
|
posx = Double.parseDouble( field[1]);
|
||||||
|
posy = Double.parseDouble( field[2]);
|
||||||
|
velx = Double.parseDouble( field[3]);
|
||||||
|
vely = Double.parseDouble( field[4]);
|
||||||
|
ratio = Double.parseDouble( field[5]);
|
||||||
|
Pwater = Double.parseDouble( field[6]);
|
||||||
|
energy = Double.parseDouble( field[7]);
|
||||||
|
pump_power = Double.parseDouble( field[8]);
|
||||||
|
simMode = Integer.parseInt( field[9]);
|
||||||
|
} catch (IOException | NullPointerException e ) {
|
||||||
|
go = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Update the display data.
|
||||||
|
rangeView.setSubmarinePos(posx, posy);
|
||||||
|
rangeView.setSubmarineVel(velx, vely);
|
||||||
|
rangeView.setBallastAirRatio(ratio);
|
||||||
|
rangeView.setWaterPressure(Pwater);
|
||||||
|
rangeView.setPumpPower(pump_power);
|
||||||
|
rangeView.setBallastEnergy(energy);
|
||||||
|
|
||||||
|
pump_power_command = rangeView.getPumpPowerCommand();
|
||||||
|
submarineDisplay.out.writeBytes( String.format("dyn.submarine.pump_power_command = %d ;\n", pump_power_command ));
|
||||||
|
|
||||||
|
submarineDisplay.out.flush();
|
||||||
|
|
||||||
|
// Update the scene.
|
||||||
|
submarineDisplay.drawRangeView();
|
||||||
|
|
||||||
|
} // while
|
||||||
|
} // main
|
||||||
|
} // class
|
||||||
@ -0,0 +1,66 @@
|
|||||||
|
/************************************************************************
|
||||||
|
PURPOSE: (Simulate a submarine.)
|
||||||
|
LIBRARY DEPENDENCIES:
|
||||||
|
((submarine/src/Submarine.o))
|
||||||
|
**************************************************************************/
|
||||||
|
#ifndef SUBMARINE_HH
|
||||||
|
#define SUBMARINE_HH
|
||||||
|
|
||||||
|
// Density of salt water = 1023.6 kg/m^3
|
||||||
|
#define DENSITY_SALT_WATER 1023.6
|
||||||
|
// Acceleration of gravity = 9.80665 m/s^2
|
||||||
|
#define GRAVITY 9.80665
|
||||||
|
//
|
||||||
|
#define CONVERSION_PASCALS_TO_PSI 1.4504e-4
|
||||||
|
|
||||||
|
class Submarine {
|
||||||
|
public:
|
||||||
|
|
||||||
|
// Parameters
|
||||||
|
double outer_hull_outer_radius;
|
||||||
|
double outer_hull_inner_radius;
|
||||||
|
double inner_hull_outer_radius;
|
||||||
|
double inner_hull_inner_radius;
|
||||||
|
double hull_length;
|
||||||
|
double ballast_tank_volume;
|
||||||
|
double hard_ballast_mass;
|
||||||
|
double hull_mass;
|
||||||
|
double payload_mass;
|
||||||
|
double Cd;
|
||||||
|
|
||||||
|
// State Variables (Uncalculated Variables)
|
||||||
|
double pos[2];
|
||||||
|
double vel[2];
|
||||||
|
double ballast_energy; // joules
|
||||||
|
double pump_power; // watts
|
||||||
|
|
||||||
|
// Calculated Variables
|
||||||
|
double acc[2];
|
||||||
|
|
||||||
|
// Control Variable
|
||||||
|
double ballast_air_ratio; // To the GUI.
|
||||||
|
double water_pressure_pascals;
|
||||||
|
double water_pressure_PSI; // To the GUI.
|
||||||
|
int pump_power_command; // From the GUI.
|
||||||
|
|
||||||
|
// Methods
|
||||||
|
int default_data();
|
||||||
|
int state_init();
|
||||||
|
int state_deriv();
|
||||||
|
int state_integ();
|
||||||
|
int control();
|
||||||
|
void calc_water_pressure();
|
||||||
|
double calc_ballast_air_volume();
|
||||||
|
double calc_pressure_hull_displaced_volume();
|
||||||
|
double calc_total_displaced_volume();
|
||||||
|
double calc_gravity_force ();
|
||||||
|
double calc_total_mass ();
|
||||||
|
double calc_fixed_mass();
|
||||||
|
double calc_buoyancy_force( );
|
||||||
|
double calc_ballast_water_mass();
|
||||||
|
double calc_ballast_water_volume ();
|
||||||
|
double calc_drag_force();
|
||||||
|
double calc_pressure_hull_volume();
|
||||||
|
int state_post_integ();
|
||||||
|
};
|
||||||
|
#endif
|
||||||
145
trick_sims/SIM_submarine/models/submarine/src/Submarine.cpp
Normal file
145
trick_sims/SIM_submarine/models/submarine/src/Submarine.cpp
Normal file
@ -0,0 +1,145 @@
|
|||||||
|
/********************************* TRICK HEADER *******************************
|
||||||
|
PURPOSE: ( Simulate a submarine. )
|
||||||
|
LIBRARY DEPENDENCY:
|
||||||
|
((Submarine.o))
|
||||||
|
*******************************************************************************/
|
||||||
|
#include "submarine/include/Submarine.hh"
|
||||||
|
|
||||||
|
#include "trick/integrator_c_intf.h"
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
int Submarine::default_data() {
|
||||||
|
|
||||||
|
// Parameters
|
||||||
|
outer_hull_outer_radius = 1.30; // meters
|
||||||
|
outer_hull_inner_radius = 1.25; // meters
|
||||||
|
inner_hull_outer_radius = 1.10; // meters
|
||||||
|
inner_hull_inner_radius = 1.00; // meters
|
||||||
|
hull_length = 3.0; // meters
|
||||||
|
ballast_tank_volume = 1.0; // cubic meter
|
||||||
|
hard_ballast_mass= 0.0; // kg
|
||||||
|
hull_mass = 10000.0; // kg
|
||||||
|
payload_mass = 1500.0; // kg
|
||||||
|
Cd = 0.5;
|
||||||
|
|
||||||
|
// State
|
||||||
|
pos[0] = 0.0; // meters
|
||||||
|
pos[1] = 0.0;
|
||||||
|
vel[0] = 0.0; // meters/second
|
||||||
|
vel[1] = 0.0;
|
||||||
|
ballast_energy = 80000.0; // Joules
|
||||||
|
pump_power = 0.0; // Watts
|
||||||
|
|
||||||
|
// Control Variable
|
||||||
|
ballast_air_ratio = 0.0;
|
||||||
|
water_pressure_pascals = 0.0;
|
||||||
|
water_pressure_PSI = 0.0;
|
||||||
|
pump_power_command = 0;
|
||||||
|
|
||||||
|
return (0);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Submarine::state_init() {
|
||||||
|
return (0);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Submarine::state_deriv() {
|
||||||
|
double total_mass = calc_total_mass();
|
||||||
|
double F_gravity = calc_gravity_force();
|
||||||
|
double F_buoyancy = calc_buoyancy_force();
|
||||||
|
double F_drag = calc_drag_force();
|
||||||
|
acc[0] = 0.0;
|
||||||
|
acc[1] = (F_gravity + F_buoyancy + F_drag) / total_mass;
|
||||||
|
return(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Submarine::state_integ() {
|
||||||
|
int integration_step;
|
||||||
|
load_state ( &pos[0], &pos[1], &vel[0], &vel[1], &ballast_energy, (double*)0);
|
||||||
|
load_deriv ( &vel[0], &vel[1], &acc[0], &acc[1], &pump_power, (double*)0);
|
||||||
|
integration_step = integrate();
|
||||||
|
unload_state( &pos[0], &pos[1], &vel[0], &vel[1], &ballast_energy, (double*)0);
|
||||||
|
if (ballast_energy < 0.0) {
|
||||||
|
ballast_energy = 0.0;
|
||||||
|
}
|
||||||
|
return(integration_step);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Calculate Water Pressure at depth in Pascals
|
||||||
|
void Submarine::calc_water_pressure() {
|
||||||
|
water_pressure_pascals = DENSITY_SALT_WATER * GRAVITY * -pos[1] + 14.7/CONVERSION_PASCALS_TO_PSI;
|
||||||
|
water_pressure_PSI = water_pressure_pascals * CONVERSION_PASCALS_TO_PSI;
|
||||||
|
}
|
||||||
|
// Calculate volume of water displaced from the ballast water tank by compressed air.
|
||||||
|
double Submarine::calc_ballast_air_volume() {
|
||||||
|
calc_water_pressure();
|
||||||
|
double ballast_air_volume = ballast_energy / water_pressure_pascals;
|
||||||
|
|
||||||
|
// Ballast_air_volume cant' exceed to volume of the ballast tank.
|
||||||
|
if ( ballast_air_volume > ballast_tank_volume ) {
|
||||||
|
ballast_air_volume = ballast_tank_volume;
|
||||||
|
ballast_energy = ballast_air_volume * water_pressure_pascals;
|
||||||
|
}
|
||||||
|
ballast_air_ratio = ballast_air_volume / ballast_tank_volume;
|
||||||
|
return ballast_air_volume;
|
||||||
|
}
|
||||||
|
// Calculate the volume of water currently displaced by the pressure hull.
|
||||||
|
// This is just multiplying the hull volume times a sigmoid.
|
||||||
|
double Submarine::calc_pressure_hull_displaced_volume() {
|
||||||
|
double C = 5.5;
|
||||||
|
return ( calc_pressure_hull_volume() / (1 + exp( C * pos[1] )));
|
||||||
|
}
|
||||||
|
|
||||||
|
double Submarine::calc_total_displaced_volume() {
|
||||||
|
return calc_ballast_air_volume() + calc_pressure_hull_displaced_volume();
|
||||||
|
}
|
||||||
|
|
||||||
|
double Submarine::calc_gravity_force () {
|
||||||
|
return -calc_total_mass() * GRAVITY;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Submarine::calc_total_mass () {
|
||||||
|
double total_mass = hard_ballast_mass + calc_fixed_mass() + calc_ballast_water_mass();
|
||||||
|
return total_mass;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Submarine::calc_fixed_mass() {
|
||||||
|
double fixed_mass = (hull_mass + payload_mass);
|
||||||
|
return fixed_mass;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Submarine::calc_buoyancy_force( ) {
|
||||||
|
return ( calc_total_displaced_volume() * DENSITY_SALT_WATER * GRAVITY );
|
||||||
|
}
|
||||||
|
|
||||||
|
double Submarine::calc_ballast_water_mass() {
|
||||||
|
double ballast_mass = calc_ballast_water_volume() * DENSITY_SALT_WATER ;
|
||||||
|
return ballast_mass;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Submarine::calc_ballast_water_volume () {
|
||||||
|
double ballast_water_volume = ballast_tank_volume - calc_ballast_air_volume();
|
||||||
|
return ballast_water_volume;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Submarine::calc_drag_force() {
|
||||||
|
double A = outer_hull_outer_radius * 2 * hull_length;
|
||||||
|
return -0.5 * DENSITY_SALT_WATER * vel[1] * fabs(vel[1]) * Cd * A;
|
||||||
|
}
|
||||||
|
|
||||||
|
double Submarine::calc_pressure_hull_volume() {
|
||||||
|
return ( M_PI * inner_hull_outer_radius * inner_hull_outer_radius * hull_length);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Submarine::control() {
|
||||||
|
pump_power = pump_power_command;
|
||||||
|
if (pump_power > 5000.0) pump_power = 5000.0;
|
||||||
|
if (pump_power < -5000.0) pump_power = -5000.0;
|
||||||
|
return(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
int Submarine::state_post_integ() {
|
||||||
|
return(0);
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user