diff --git a/trick_sims/SIM_aircraft/Modified_data/default.waypoints b/trick_sims/SIM_aircraft/Modified_data/default.waypoints new file mode 100644 index 00000000..8ef781ef --- /dev/null +++ b/trick_sims/SIM_aircraft/Modified_data/default.waypoints @@ -0,0 +1,6 @@ + 0.0, 25000.0,images/wp0.png + 21650.0, 12500.0,images/wp1.png + 21650.0, -12500.0,images/wp2.png + 0.0, -25000.0,images/wp3.png +-21650.0, -12500.0,images/wp4.png +-21650.0, 12500.0,images/wp5.png \ No newline at end of file diff --git a/trick_sims/SIM_aircraft/README.md b/trick_sims/SIM_aircraft/README.md index b5b0bbad..b05ea61b 100644 --- a/trick_sims/SIM_aircraft/README.md +++ b/trick_sims/SIM_aircraft/README.md @@ -16,7 +16,7 @@ In this simulation, we can manually control the aircraft by specifying: or automatically. ## Building the Simulation -In the ```SIM_aircrafte``` directory, type **```trick-CP```** to build the simulation executable. When it's complete, you should see: +In the ```SIM_aircraft``` directory, type **```trick-CP```** to build the simulation executable. When it's complete, you should see: ``` === Simulation make complete === @@ -32,9 +32,36 @@ In the SIM_aircraft directory: The Sim Control Panel, and a graphics client called "Aircraft Display" should appear. Click Start on the Trick Sim Control Panel. -q + ![](images/GraphicsClient.png) +### Manual Control of the Aircraft +The two sliders on the bottom left and right are for setting the aircraft's desired speed and heading, respectively. The desired speed can range from 0 to 250 meters per second. The desired heading is measured in radians, from - $\pi$ to $\pi$. + +### Aircraft Autopilot +The autopilot feature is toggled on and off by the ```Autopilot OFF/ON``` button at the bottom of the graphics client. + +![](images/GraphicsClientAutopilot.png) + +When active, the aircraft's heading is automatically calculated to approach a series of waypoints. These waypoints are marked on the map by the image provided in the input file. + +#### Adding Waypoints with an Input File +Waypoints can be added to the simulation using an input file. + +![](images/WaypointInputFile.png) + +When the simulation starts they are read into the simulation from the specified file. Currently that file is `default.waypoints` within the `Modified_data` folder. + +#### Sample Input in [`default.waypoints`](Modified\_data/default.waypoints): + + 0.0, 25000.0,images/wp0.png + 21650.0, 12500.0,images/wp1.png + 21650.0, -12500.0,images/wp2.png + 0.0, -25000.0,images/wp3.png + -21650.0, -12500.0,images/wp4.png + -21650.0, 12500.0,images/wp5.png +Each line should have three pieces of data, separated by commas. The first two are the coordinates. They are formatted like GPS coordinates, with the first number being the vertical distance, or 'North', and the second being the horizontal, or 'West', distance. The third part of the line is the path to the image that will be used as the waypoint marker. + ## Dynamics Model ### General Strategy for Motion @@ -93,25 +120,25 @@ So, we must limit $thrust_{actual}$ to that which our engine can actually produc ![](images/Equation_6.png) ![](images/Equation_7.png) -So, the magnitude of the thrust force vector is $thrust_{actual}$ , and the direction is [1,0] in Body coordinates. +So, the magnitude of the thrust force vector is $thrust_{actual}$ , and the direction is [1,0] in Body coordinates. ### Aircraft Heading -If the aircraft is moving directly north, $\psi = 0$ . Directly west is $\pi/2$ . Directly east is $-\pi/2$ . +If the aircraft is moving directly north, $\psi = 0$ . Directly west is $\pi/2$ . Directly east is $-\pi/2$ . ![](images/AircraftHeading.png) ### Heading Control -To control our heading, we’d like to calculate a heading rate that is proportional to the difference between our current heading and our desired heading. +To control our heading, we’d like to calculate a heading rate that is proportional to the difference between our current heading and our desired heading. ![](images/HeadingControl.png) ![](images/Equation_8.png) -Notice (in the figure) that $\psi_{diff}$ is in the direction that we want to turn. But, what if ( $\left| \psi_{diff} \right| > \pi$ )? That is, it’s greater than 180 degrees? +Notice (in the figure) that $\psi_{diff}$ is in the direction that we want to turn. But, what if ( $\left| \psi_{diff} \right| > \pi$ )? That is, it’s greater than 180 degrees? -Do you ever have to turn more than 𝜋 radians (180 degrees) to face any direction? No, you don’t. +Do you ever have to turn more than 𝜋 radians (180 degrees) to face any direction? No, you don’t. Let’s now define $\pis_{error}$ as the actual direction of turning. @@ -121,19 +148,19 @@ This is how we will calculate our desired heading rate: ![](images/Equation_10.png) -That is, our desired heading rate is proportional to the amount and direction that we need to turn. +That is, our desired heading rate is proportional to the amount and direction that we need to turn. $G_{\psi}$ is a proportionality constant. ### Turning Force -The turning force we needed to turn the desired rate is : +The turning force we needed to turn the desired rate is : ![](images/Equation_11.png) -Like our desired speed, just we want something doesn’t mean that the aircraft is capable of it. In our case we need to limit the magnitude of our force to 1 G ( force of gravity), that is 9.8 m/s2 * m. So if our mass is 5000 kg, we need to limit our turning force to between -49000 .. 49000 Newtons. +Like our desired speed, just we want something doesn’t mean that the aircraft is capable of it. In our case we need to limit the magnitude of our force to 1 G ( force of gravity), that is 9.8 m/s2 * m. So if our mass is 5000 kg, we need to limit our turning force to between -49000 .. 49000 Newtons. -But we need that total force in world coordinates so we can move our aircraft around in the world. -So, if our heading of our aircraft is 𝜓, what is our direction vector expressed in world coordinates? +But we need that total force in world coordinates so we can move our aircraft around in the world. +So, if our heading of our aircraft is 𝜓, what is our direction vector expressed in world coordinates? ![](images/Equation_12.png) \ No newline at end of file diff --git a/trick_sims/SIM_aircraft/RUN_test/input.py b/trick_sims/SIM_aircraft/RUN_test/input.py index 5d3c19e6..3534ad46 100644 --- a/trick_sims/SIM_aircraft/RUN_test/input.py +++ b/trick_sims/SIM_aircraft/RUN_test/input.py @@ -7,6 +7,17 @@ dyn.aircraft.set_desired_compass_heading(45.0); dyn.aircraft.autoPilot = True; dyn.aircraft.desired_speed = 200 # meters per second +#========================================== +# Read the waypoints from the input file +#========================================== + +waypoints_path = "Modified_data/default.waypoints" +fp = open(waypoints_path, "r") +for line in fp: + fields = line.split(",") + dyn.aircraft.add_waypoint( float(fields[0]), float(fields[1])) +dyn.aircraft.reset_trip() + #========================================== # Start the Satellite Graphics Client #========================================== @@ -16,6 +27,7 @@ AircraftDisplay_path = "models/graphics/build/AircraftDisplay.jar" if (os.path.isfile(AircraftDisplay_path)) : AircraftDisplay_cmd = "java -jar " \ + AircraftDisplay_path \ + + " -w " + waypoints_path \ + " " + str(varServerPort) + " &" ; print(AircraftDisplay_cmd) os.system( AircraftDisplay_cmd); diff --git a/trick_sims/SIM_aircraft/images/GraphicsClientAutopilot.png b/trick_sims/SIM_aircraft/images/GraphicsClientAutopilot.png new file mode 100644 index 00000000..a32e93ae Binary files /dev/null and b/trick_sims/SIM_aircraft/images/GraphicsClientAutopilot.png differ diff --git a/trick_sims/SIM_aircraft/images/WaypointInputFile.png b/trick_sims/SIM_aircraft/images/WaypointInputFile.png new file mode 100644 index 00000000..0118a5c8 Binary files /dev/null and b/trick_sims/SIM_aircraft/images/WaypointInputFile.png differ diff --git a/trick_sims/SIM_aircraft/images/wp0.png b/trick_sims/SIM_aircraft/images/wp0.png new file mode 100644 index 00000000..c7fb54b4 Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp0.png differ diff --git a/trick_sims/SIM_aircraft/images/wp1.png b/trick_sims/SIM_aircraft/images/wp1.png new file mode 100644 index 00000000..ce828ed8 Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp1.png differ diff --git a/trick_sims/SIM_aircraft/images/wp10.png b/trick_sims/SIM_aircraft/images/wp10.png new file mode 100644 index 00000000..504b111f Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp10.png differ diff --git a/trick_sims/SIM_aircraft/images/wp11.png b/trick_sims/SIM_aircraft/images/wp11.png new file mode 100644 index 00000000..53d26eb3 Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp11.png differ diff --git a/trick_sims/SIM_aircraft/images/wp2.png b/trick_sims/SIM_aircraft/images/wp2.png new file mode 100644 index 00000000..ccbf4d44 Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp2.png differ diff --git a/trick_sims/SIM_aircraft/images/wp3.png b/trick_sims/SIM_aircraft/images/wp3.png new file mode 100644 index 00000000..14e44780 Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp3.png differ diff --git a/trick_sims/SIM_aircraft/images/wp4.png b/trick_sims/SIM_aircraft/images/wp4.png new file mode 100644 index 00000000..74ea4681 Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp4.png differ diff --git a/trick_sims/SIM_aircraft/images/wp5.png b/trick_sims/SIM_aircraft/images/wp5.png new file mode 100644 index 00000000..f411fab6 Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp5.png differ diff --git a/trick_sims/SIM_aircraft/images/wp6.png b/trick_sims/SIM_aircraft/images/wp6.png new file mode 100644 index 00000000..beee900e Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp6.png differ diff --git a/trick_sims/SIM_aircraft/images/wp7.png b/trick_sims/SIM_aircraft/images/wp7.png new file mode 100644 index 00000000..95ce3a8c Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp7.png differ diff --git a/trick_sims/SIM_aircraft/images/wp8.png b/trick_sims/SIM_aircraft/images/wp8.png new file mode 100644 index 00000000..e7f376bf Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp8.png differ diff --git a/trick_sims/SIM_aircraft/images/wp9.png b/trick_sims/SIM_aircraft/images/wp9.png new file mode 100644 index 00000000..d6b98d36 Binary files /dev/null and b/trick_sims/SIM_aircraft/images/wp9.png differ diff --git a/trick_sims/SIM_aircraft/models/Aircraft/include/Aircraft.hh b/trick_sims/SIM_aircraft/models/Aircraft/include/Aircraft.hh index a6853a8e..6485bdc7 100644 --- a/trick_sims/SIM_aircraft/models/Aircraft/include/Aircraft.hh +++ b/trick_sims/SIM_aircraft/models/Aircraft/include/Aircraft.hh @@ -6,6 +6,13 @@ LIBRARY DEPENDENCIES: #ifndef Aircraft_HH #define Aircraft_HH +#include + +class WayPoint { + public: + double pos[2]; +}; + class Aircraft { public: // Calculated by numeric integration of state derivatives. @@ -13,7 +20,8 @@ class Aircraft { double vel[2]; // Updated by control logic; - int current_waypoint; + std::vector waypointQueue; + std::vector::iterator current_waypoint; // Static Parameters (Don't change during the simulation.) double mass; @@ -40,6 +48,8 @@ class Aircraft { int control(); void set_desired_compass_heading( double compass_heading); + void add_waypoint(double n, double w); + void reset_trip(); void calc_total_force( double (&F_total_body)[2]); void calc_drag_force( double (&F_drag_body)[2]); diff --git a/trick_sims/SIM_aircraft/models/Aircraft/src/Aircraft.cpp b/trick_sims/SIM_aircraft/models/Aircraft/src/Aircraft.cpp index aed07230..0178560a 100644 --- a/trick_sims/SIM_aircraft/models/Aircraft/src/Aircraft.cpp +++ b/trick_sims/SIM_aircraft/models/Aircraft/src/Aircraft.cpp @@ -10,28 +10,12 @@ LIBRARY DEPENDENCY: #include #include -class WayPoint { - public: - double pos[2]; -}; - -// Waypoints are in World Coordinates (West, North). -#define NUMBER_OF_WAYPOINTS 6 -WayPoint waypoint[ NUMBER_OF_WAYPOINTS ] = { - { 0.0, 25000.0}, - { 21650.0, 12500.0}, - { 21650.0, -12500.0}, - { 0.0, -25000.0}, - {-21650.0, -12500.0}, - {-21650.0, 12500.0} -}; - int Aircraft::default_data() { pos[0] = 0.0; // m pos[1] = 0.0; vel[0] = 100.0; // m/s vel[1] = 0.0; - current_waypoint = 0; + mass = 5000; // kg thrust_mag_max = 45000; // N K_drag = 0.72; @@ -39,7 +23,7 @@ int Aircraft::default_data() { set_desired_compass_heading(45.0); desired_speed = 200; // m/s autoPilot = false; - current_waypoint = 0; + current_waypoint = waypointQueue.begin(); return (0); } @@ -140,21 +124,34 @@ void Aircraft::rotateBodyToWorld( double (&F_total_world)[2], double (&F_total_b F_total_world[1] = sin(heading) * F_total_body[0] + cos(heading) * F_total_body[1]; } +void Aircraft::reset_trip() { + current_waypoint = waypointQueue.begin(); +} + +void Aircraft::add_waypoint(double n, double w) { + WayPoint wp = { {n, w} }; + waypointQueue.push_back(wp); +} + int Aircraft::control() { if (autoPilot) { - if (NUMBER_OF_WAYPOINTS > 0) { + if (waypointQueue.size() > 0) { // Calculate the difference between where we want to be, and where we are. double posDiff[2]; - vector_difference(posDiff, waypoint[current_waypoint].pos, pos); + vector_difference(posDiff, current_waypoint->pos, pos); // Calculate bearing to waypoint. desired_heading = northWestToPsi(posDiff); // Calculate distance to waypoint. double distanceToWaypoint = vector_magnitude(posDiff); // If we've arrived, that is we're close enough, go to the next waypoint. if (distanceToWaypoint < 100.0) { - current_waypoint ++; - current_waypoint = current_waypoint % NUMBER_OF_WAYPOINTS; + ++current_waypoint; + if (current_waypoint == waypointQueue.end()) { + current_waypoint = waypointQueue.begin(); + } } + } else { + std::cout << "!!! Waypoint List Empty !!!" << std::endl; } } return 0; diff --git a/trick_sims/SIM_aircraft/models/graphics/Makefile b/trick_sims/SIM_aircraft/models/graphics/Makefile index d3e0cbc0..51450f81 100755 --- a/trick_sims/SIM_aircraft/models/graphics/Makefile +++ b/trick_sims/SIM_aircraft/models/graphics/Makefile @@ -3,4 +3,4 @@ all: mvn package clean: - rm -rf build + rm -rf build \ No newline at end of file diff --git a/trick_sims/SIM_aircraft/models/graphics/pom.xml b/trick_sims/SIM_aircraft/models/graphics/pom.xml index d3d3e1d9..d63b61d0 100644 --- a/trick_sims/SIM_aircraft/models/graphics/pom.xml +++ b/trick_sims/SIM_aircraft/models/graphics/pom.xml @@ -118,4 +118,4 @@ - + \ No newline at end of file diff --git a/trick_sims/SIM_aircraft/models/graphics/src/main/java/trick/aircraftDisplay/AircraftDisplay.java b/trick_sims/SIM_aircraft/models/graphics/src/main/java/trick/aircraftDisplay/AircraftDisplay.java index 16eae817..1d2e6822 100644 --- a/trick_sims/SIM_aircraft/models/graphics/src/main/java/trick/aircraftDisplay/AircraftDisplay.java +++ b/trick_sims/SIM_aircraft/models/graphics/src/main/java/trick/aircraftDisplay/AircraftDisplay.java @@ -13,6 +13,7 @@ import java.awt.image.AffineTransformOp; import java.awt.image.BufferedImage; import java.awt.event.ActionListener; import java.awt.event.ActionEvent; +import java.awt.Toolkit; import javax.swing.event.ChangeEvent; import javax.swing.event.ChangeListener; import java.awt.event.ItemEvent; @@ -24,6 +25,7 @@ import java.io.DataOutputStream; import java.io.IOException; import java.io.InputStreamReader; import java.io.FileReader; +import java.io.FileNotFoundException; import java.lang.Math; import java.net.Socket; import java.util.*; @@ -40,11 +42,25 @@ import javax.swing.border.EtchedBorder; import javax.swing.BoxLayout; import java.awt.Color; +import javax.imageio.ImageIO; +import java.io.File; + /** * * @author penn */ +class Waypoint { + public double north, west; + public BufferedImage icon; + + public Waypoint(double n, double w, BufferedImage i) { + north = n; + west = w; + icon = i; + } +} + class ScenePoly { public Color color; public int n; @@ -70,6 +86,8 @@ class SkyView extends JPanel { private double desired_heading; private Boolean autopilot; + private ArrayList waypoints; + // Origin of world coordinates in jpanel coordinates. private int worldOriginX; private int worldOriginY; @@ -97,6 +115,19 @@ class SkyView extends JPanel { workPolyX = new int[30]; workPolyY = new int[30]; + + waypoints = new ArrayList(); + } + + public void addWaypoint( double n, double w, String fp) { + BufferedImage img; + try { + img = ImageIO.read(new File(fp)); + waypoints.add(new Waypoint(n,w,img)); + } catch(Exception e) { + System.out.printf("Waypoint (%.1f,%.1f) not added to map."); + } + } public void setAircraftPos( double n, double w) { @@ -166,6 +197,13 @@ class SkyView extends JPanel { g.fillOval(x,y,r,r); } + public void drawWaypoint(Graphics2D g, Waypoint wp) { + int x = 0, y = 0; + x = (int)((worldOriginX - scale * wp.west) - (wp.icon.getWidth()/2)); + y = (int)((worldOriginY - scale * wp.north) - (wp.icon.getHeight()/2)); + g.drawImage(wp.icon, x, y, null); + } + public void drawScenePoly(Graphics2D g, ScenePoly p, double angle_r , double north, double west) { double mag = 800.0; @@ -196,12 +234,10 @@ class SkyView extends JPanel { g2d.drawLine( worldOriginX, 0, worldOriginX, height); // Draw Waypoints - drawScenePoly(g2d, wpmarker, 0.0, 0.0, 25000.0 ); - drawScenePoly(g2d, wpmarker, 0.0, 21650.0, 12500.0 ); - drawScenePoly(g2d, wpmarker, 0.0, 21650.0, -12500.0 ); - drawScenePoly(g2d, wpmarker, 0.0, 0.0, -25000.0 ); - drawScenePoly(g2d, wpmarker, 0.0, -21650.0, -12500.0 ); - drawScenePoly(g2d, wpmarker, 0.0, -21650.0, 12500.0 ); + for(int i = 0; i < waypoints.size(); i++) { + Waypoint wp = waypoints.get(i); + drawWaypoint(g2d, wp); + } // Draw Aircraft drawScenePoly(g2d, aircraft, heading, aircraftPos[0], aircraftPos[1] ); @@ -441,7 +477,7 @@ public class AircraftDisplay extends JFrame { String host = "localHost"; int port = 0; - String vehicleImageFile = null; + String waypointInputFile = null; int ii = 0; while (ii < args.length) { @@ -451,6 +487,10 @@ public class AircraftDisplay extends JFrame { printHelpText(); System.exit(0); } break; + case "-w" : + case "--waypoints" : { + waypointInputFile = args[++ii]; + } break; default : { port = (Integer.parseInt(args[ii])); } break; @@ -464,6 +504,12 @@ public class AircraftDisplay extends JFrame { System.exit(0); } + if(waypointInputFile == null) { + System.out.println("No waypoint file specified"); + printHelpText(); + System.exit(0); + } + double mapScale = 0.01; // 100 meters per pixel SkyView skyview = new SkyView( mapScale); AircraftDisplay sd = new AircraftDisplay(skyview); @@ -478,6 +524,18 @@ public class AircraftDisplay extends JFrame { Boolean autopilot = false; double desired_heading = 0.0; + try { + BufferedReader br = new BufferedReader(new FileReader(waypointInputFile)); + String line; + while((line = br.readLine()) != null) { + String[] parsedLine = line.split(","); + sd.skyView.addWaypoint(Double.parseDouble(parsedLine[0]), Double.parseDouble(parsedLine[1]), parsedLine[2]); + } + } catch(FileNotFoundException e) { + System.out.printf("'%s' not found", args[ii+1]); + System.exit(0); + } + System.out.println("Connecting to: " + host + ":" + port); sd.connectToServer(host, port);