From 2a8c1ea0005329a3d8bb71b2dac9fceac3ffb07d Mon Sep 17 00:00:00 2001 From: Max Schrader Date: Thu, 14 Jan 2021 00:14:56 -0500 Subject: [PATCH 01/23] Added Javadoc as well as untested extension calc --- .../ORBrake/ORBrakeSimulationListener.java | 89 +++++++++++++++++-- 1 file changed, 83 insertions(+), 6 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 316d0ac..81cacf9 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -1,6 +1,7 @@ package net.sf.openrocket.ORBrake; import java.lang.Math; +import java.util.stream.IntStream; import net.sf.openrocket.simulation.SimulationStatus; import net.sf.openrocket.simulation.exception.SimulationException; import net.sf.openrocket.simulation.listeners.AbstractSimulationListener; @@ -9,8 +10,16 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { private double velocity; - private double drag_coef = 1.3; - private double density = 1.225; + private double altitude; + + private double surfConst[][] = { // Surface constants for presimulated airbrake extensions. + {-0.000000000, 0.000000000, -0.000000000, 0.0000000000, 0.000000000}, // 0 % + {-0.046526224, 0.000020669, -0.000299962, -0.0000001251, 0.000010806}, // 20 % + {-0.093052324, 0.000041339, -0.000599922, -0.0000002503, 0.000021611}, // 40 % + {-0.139578485, 0.000062008, -0.000899883, -0.0000003754, 0.000032417}, // 60 % + {-0.186104574, 0.000082677, -0.001199843, -0.0000005006, 0.000043222}, // 80 % + {-0.232630682, 0.000103346, -0.001499803, -0.0000006257, 0.000054028} // 100% + }; public ORBrakeSimulationListener() { super(); @@ -18,21 +27,89 @@ public ORBrakeSimulationListener() { @Override public FlightConditions postFlightConditions(SimulationStatus status, FlightConditions conditions) + /** + * Called immediately after the flight conditions of the current time + * step so that relevant ones can be extracted. + * + * @param status Object that contains simulation status details. + * @param conditions Object that contains flight condition details. + * @return null. + */ { velocity = conditions.getVelocity(); return null; } @Override - public double postSimpleThrustCalculation(SimulationStatus status, double thrust) throws SimulationException + public double postSimpleThrustCalculation(SimulationStatus status, double thrust) // throws SimulationException + /** + * Influences the thrust after it is computed at each time step + * but before it is applied to the vehicle. + * + * @param status Object that contains simulation status details. + * @param thrust The computed motor thrust. + * @return The modified thrust to be actually applied. + */ { // if (status.getSimulationTime() ) - return thrust + AirbrakeForce(velocity); + return thrust + AirbrakeForce(); + } + + private double AirbrakeForce() + { + double requiredDrag = 3; + return requiredDrag; + //return drag_coef * density * Math.pow(vel, 2) / 2; + } + + private double ExtensionFromDrag(double requiredDrag) + /** + * Computes the required extension to achieve a required drag. + * + * @param requiredDrag The desired drag from the control system. + * @return The percentage deployment that will produce that drag. + */ + { + double[] drag = new double[6]; + + // Compute drag for each known extension. + IntStream.range(0, 5).forEachOrdered(n -> { + drag[n] = this.DragSurface(n); + }); + + // Interpolate to find desired extension + double extension = 0; + double term; + + for (int i = 0; i < 5; ++i) + { + term = i; + for (int j = 0; j < 5; ++j) + { + if(j != i) + { + term *= (requiredDrag - drag[j]) / (drag[i] - drag[j]); + } + }; + + extension += term; + }; + extension *= 20; + return extension; } - private double AirbrakeForce(double vel) + private double DragSurface(int extNum) + /** + * Finds the drag force at one of the 6 drag surfaces given the current + * velocity and altitude. + * + * @param extNum The index of the extension percentage. Each increment + * represents a 20% increase. + * @return The drag given altitude, velocity, and extension. + */ { - return drag_coef * density * Math.pow(vel, 2) / 2; + return surfConst[extNum][0] + surfConst[extNum][1]*altitude + surfConst[extNum][2]*velocity + + surfConst[extNum][3]*altitude*velocity + surfConst[extNum][4]*Math.pow(velocity, 2); } } \ No newline at end of file From 24074ae96f3813a715b3fb5da1a459effcff8130 Mon Sep 17 00:00:00 2001 From: Max Schrader Date: Thu, 14 Jan 2021 22:39:31 -0500 Subject: [PATCH 02/23] Implimented Junit --- .../ORBrake/ORBrakeSimulationListener.java | 16 ++++---- .../sf/openrocket/ORBrake/ORBrakeTest.java | 38 +++++++++++++++++++ 2 files changed, 46 insertions(+), 8 deletions(-) create mode 100644 src/net/sf/openrocket/ORBrake/ORBrakeTest.java diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 81cacf9..2b1ea1c 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -9,10 +9,10 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { - private double velocity; - private double altitude; + double velocity; + double altitude; - private double surfConst[][] = { // Surface constants for presimulated airbrake extensions. + private static final double surfConst[][] = { // Surface constants for presimulated airbrake extensions. {-0.000000000, 0.000000000, -0.000000000, 0.0000000000, 0.000000000}, // 0 % {-0.046526224, 0.000020669, -0.000299962, -0.0000001251, 0.000010806}, // 20 % {-0.093052324, 0.000041339, -0.000599922, -0.0000002503, 0.000021611}, // 40 % @@ -52,17 +52,17 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust */ { // if (status.getSimulationTime() ) - return thrust + AirbrakeForce(); + return thrust + airbrakeForce(); } - private double AirbrakeForce() + double airbrakeForce() { double requiredDrag = 3; return requiredDrag; //return drag_coef * density * Math.pow(vel, 2) / 2; } - private double ExtensionFromDrag(double requiredDrag) + double extensionFromDrag(double requiredDrag) /** * Computes the required extension to achieve a required drag. * @@ -74,7 +74,7 @@ private double ExtensionFromDrag(double requiredDrag) // Compute drag for each known extension. IntStream.range(0, 5).forEachOrdered(n -> { - drag[n] = this.DragSurface(n); + drag[n] = this.dragSurface(n); }); // Interpolate to find desired extension @@ -98,7 +98,7 @@ private double ExtensionFromDrag(double requiredDrag) return extension; } - private double DragSurface(int extNum) + double dragSurface(int extNum) /** * Finds the drag force at one of the 6 drag surfaces given the current * velocity and altitude. diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java new file mode 100644 index 0000000..9ce54c1 --- /dev/null +++ b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java @@ -0,0 +1,38 @@ +package net.sf.openrocket.ORBrake; + +import static org.junit.jupiter.api.Assertions.*; + +import org.junit.jupiter.api.AfterAll; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class ORBrakeTest { + + static ORBrakeSimulationListener listener; + + @BeforeAll + static void setUpBeforeClass() throws Exception { + listener = new ORBrakeSimulationListener(); + } + +// @AfterAll +// static void tearDownAfterClass() throws Exception { +// } + + @BeforeEach + void setUp() throws Exception { + listener.velocity = 300; + listener.altitude = 3000; + } + +// @AfterEach +// void tearDown() throws Exception { +// } + + @Test + void dragSurfaceTest() { + assertEquals(listener.dragSurface(5), 5555, .0001); + } +} From 2e3dbe626fd37d444090912de8932a4b9c55d24c Mon Sep 17 00:00:00 2001 From: Max Schrader Date: Fri, 15 Jan 2021 18:40:01 -0500 Subject: [PATCH 03/23] Replaced Placeholder Surface Constants w/ Real Data --- .../openrocket/ORBrake/ORBrakeSimulationListener.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 2b1ea1c..a3038c7 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -14,11 +14,11 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { private static final double surfConst[][] = { // Surface constants for presimulated airbrake extensions. {-0.000000000, 0.000000000, -0.000000000, 0.0000000000, 0.000000000}, // 0 % - {-0.046526224, 0.000020669, -0.000299962, -0.0000001251, 0.000010806}, // 20 % - {-0.093052324, 0.000041339, -0.000599922, -0.0000002503, 0.000021611}, // 40 % - {-0.139578485, 0.000062008, -0.000899883, -0.0000003754, 0.000032417}, // 60 % - {-0.186104574, 0.000082677, -0.001199843, -0.0000005006, 0.000043222}, // 80 % - {-0.232630682, 0.000103346, -0.001499803, -0.0000006257, 0.000054028} // 100% + {-0.023136854, 0.000010395, 0.000071445, -0.0000000862, 0.000008669}, // 20 % + {-0.041211681, 0.000017555, 0.000565077, -0.0000002520, 0.000017995}, // 40 % + {-0.085142430, 0.000038009, 0.000316569, -0.0000003705, 0.000029817}, // 60 % + {-0.152819731, 0.000068195, 0.000074012, -0.0000004884, 0.000043697}, // 80 % + {-0.261060050, 0.000115821, -0.000232887, -0.0000007869, 0.000061334} // 100% }; public ORBrakeSimulationListener() { From a3237bcff567174ea186c53ed0d5bc0327345839 Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Sun, 17 Jan 2021 01:38:29 -0500 Subject: [PATCH 04/23] Added some pseudocode for PID controller (unfinished) --- .../ORBrake/ORBrakeSimulationListener.java | 55 +++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 2b1ea1c..ad13dde 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -62,6 +62,61 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust //return drag_coef * density * Math.pow(vel, 2) / 2; } + // START OF PSEUDOCODE + + // Define variables + + // Error function variables + double err; //error + double SP; //setpoint + double measure; //measurement + + // PID controller gain constants + double Kp; + double Ki; + double Kd; + + // Low pass filter time constant + double tau; + + // Output limits + double min; + double max; + + // Sample time in seconds + double T; + + double prop; //proportional + + // Memory + double inte; //integral + double prev_err; //previous error + double diff; //differential + double prev_measure; //previous measurement + + // Output + double out; + + public PID_initial(variables) + { + inte = 0; + prev_err = 0; + diff = 0; + prev_measure = 0; + out = 0; + } + public PID_update(variables) + { + err = setpoint - measure; + prop = Kp*err; + inte = 0.5*Ki*T*(err+prev_err) + inte; + // [clamp integrator here] + diff = ( 2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); + out = prop + inte + diff; + } + + // END OF PSEUDOCODE + double extensionFromDrag(double requiredDrag) /** * Computes the required extension to achieve a required drag. From 9d18d55dfc372529fe60fed19c5a155bcbab4356 Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Sun, 17 Jan 2021 17:57:57 -0500 Subject: [PATCH 05/23] Updated PID controller code --- .../ORBrake/ORBrakeSimulationListener.java | 115 +++++++++++------- 1 file changed, 74 insertions(+), 41 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index ad13dde..0973609 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -62,60 +62,93 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust //return drag_coef * density * Math.pow(vel, 2) / 2; } - // START OF PSEUDOCODE - - // Define variables - - // Error function variables - double err; //error - double SP; //setpoint - double measure; //measurement - - // PID controller gain constants - double Kp; - double Ki; - double Kd; - - // Low pass filter time constant - double tau; - - // Output limits - double min; - double max; - - // Sample time in seconds - double T; - - double prop; //proportional - - // Memory - double inte; //integral - double prev_err; //previous error - double diff; //differential - double prev_measure; //previous measurement + // PID CONTROLLER + + void struct() + { + double Kp; //proportional gain constant + double Ki; //integral gain constant + double Kd; //derivative gain constant - // Output - double out; + double tau; //low pass filter time constant - public PID_initial(variables) + double min; //output min limit + double max; //output max limit + double min_inte; //integral min limit + double max_inte; //integral max limit + + double T; //sample time in sec + + // Memory + double inte; //integral term + double prev_err; //previous error + double diff; //differential term + double prev_measure; //previous measurement + + double out; //output + } parameters + + void PID_initial(parameters) { inte = 0; prev_err = 0; diff = 0; prev_measure = 0; + out = 0; } - public PID_update(variables) - { - err = setpoint - measure; - prop = Kp*err; + + double PID_update(parameters, double SP, double measure) + { + // Error function + double err = SP - measure; + /** + * SP = setpoint, desired angle of airbrake extension + * measure = actual measurement, current angle of airbrake extension + */ + + // Proportional Term + double prop = Kp*err; + + // Integral Term inte = 0.5*Ki*T*(err+prev_err) + inte; - // [clamp integrator here] - diff = ( 2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); + + // Anti-wind up (static integral clamping) +// if (max > prop) { +// max_inte = max - prop; +// } else { +// max_inte = 0; +// } +// if (min < prop) { +// min_inte = min - prop; +// } else { +// min_inte = 0; +// } + if (inte > max_inte) { + inte = max_inte; + } else if (inte < min_inte) { + inte = min_inte; + } + + // Differential Term + diff = -( 2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); + + // Output out = prop + inte + diff; + if (out > max) { + out = max; + } else if (out < min) { + out = min; + } + + // Update memory + prev_err = error; + prev_measure = measure; + + return out; } - // END OF PSEUDOCODE + // END OF PID CONTROLLER CODE double extensionFromDrag(double requiredDrag) /** From ace5b98c7087079b2b1fe3dc616679ea56337a95 Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Sun, 17 Jan 2021 18:16:45 -0500 Subject: [PATCH 06/23] Added PID control loop --- .../ORBrake/ORBrakeSimulationListener.java | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 0973609..d7f4f93 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -96,6 +96,8 @@ void PID_initial(parameters) prev_measure = 0; out = 0; + + cont = 1; } double PID_update(parameters, double SP, double measure) @@ -148,6 +150,19 @@ void PID_initial(parameters) return out; } + PID_initial(parameters); + while (cont = true) + { + measure = out; + PID_update(parameters, SP, measure); + + if (SP = out) { + cont = 0; + } else { + cont = 1; + } + } + // END OF PID CONTROLLER CODE double extensionFromDrag(double requiredDrag) From 8c0c8519a681a532ff9808ccdfe795223a914c4b Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Sun, 17 Jan 2021 18:19:23 -0500 Subject: [PATCH 07/23] Updated formatting of PID parameters --- src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index d7f4f93..cf8482f 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -66,7 +66,9 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust void struct() { - double Kp; //proportional gain constant + // Input parameters + + double Kp; //proportional gain constant double Ki; //integral gain constant double Kd; //derivative gain constant @@ -80,6 +82,7 @@ void struct() double T; //sample time in sec // Memory + double inte; //integral term double prev_err; //previous error double diff; //differential term From 6de579a2d7fdc1c29ca3e56bf3b23df41e438c9f Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Mon, 18 Jan 2021 00:00:49 -0500 Subject: [PATCH 08/23] Reorganized PID controller code --- .../ORBrake/ORBrakeSimulationListener.java | 165 ++++++++---------- 1 file changed, 77 insertions(+), 88 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index cf8482f..c5be8da 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -64,35 +64,40 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust // PID CONTROLLER - void struct() - { - // Input parameters - - double Kp; //proportional gain constant - double Ki; //integral gain constant - double Kd; //derivative gain constant + // Calculated parameters + + double err; //error + double SP; //setpoint (desired altitude) + double measure; //actual measurement (predicted altitude) + double requiredDrag; //required drag + + // Input parameters + + double Kp = 1; //proportional gain constant + double Ki = 1; //integral gain constant + double Kd = 1; //derivative gain constant - double tau; //low pass filter time constant + double tau = 1; //low pass filter time constant - double min; //output min limit - double max; //output max limit - double min_inte; //integral min limit - double max_inte; //integral max limit - - double T; //sample time in sec - - // Memory - - double inte; //integral term - double prev_err; //previous error - double diff; //differential term - double prev_measure; //previous measurement + double min = 1; //output min limit + double max = 5; //output max limit + double min_inte = 1; //integral min limit + double max_inte = 5; //integral max limit + + double T = 1; //sample time in sec + + // Memory variables + + double inte; //integral term + double prev_err; //previous error + double diff; //differential term + double prev_measure; //previous measurement - double out; //output - } parameters + double out; //output - void PID_initial(parameters) + double PID_controller() { + // Initial conditions inte = 0; prev_err = 0; diff = 0; @@ -100,70 +105,54 @@ void PID_initial(parameters) out = 0; - cont = 1; - } - - double PID_update(parameters, double SP, double measure) - { - // Error function - double err = SP - measure; - /** - * SP = setpoint, desired angle of airbrake extension - * measure = actual measurement, current angle of airbrake extension - */ - - // Proportional Term - double prop = Kp*err; - - // Integral Term - inte = 0.5*Ki*T*(err+prev_err) + inte; - - // Anti-wind up (static integral clamping) -// if (max > prop) { -// max_inte = max - prop; -// } else { -// max_inte = 0; -// } -// if (min < prop) { -// min_inte = min - prop; -// } else { -// min_inte = 0; -// } - if (inte > max_inte) { - inte = max_inte; - } else if (inte < min_inte) { - inte = min_inte; - } - - // Differential Term - diff = -( 2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); - - // Output - out = prop + inte + diff; - if (out > max) { - out = max; - } else if (out < min) { - out = min; - } - - // Update memory - prev_err = error; - prev_measure = measure; - - return out; - } - - PID_initial(parameters); - while (cont = true) - { - measure = out; - PID_update(parameters, SP, measure); - - if (SP = out) { - cont = 0; - } else { - cont = 1; - } + while (SP - prev_measure > 0.01) + { + measure = out; + + // Error function + double err = SP - measure; + + // Proportional term + double prop = Kp*err; + + // Integral term + inte = 0.5*Ki*T*(err+prev_err) + inte; + + // Anti-wind up (static integral clamping) +// if (max > prop) { +// max_inte = max - prop; +// } else { +// max_inte = 0; +// } +// if (min < prop) { +// min_inte = min - prop; +// } else { +// min_inte = 0; +// } + if (inte > max_inte) { + inte = max_inte; + } else if (inte < min_inte) { + inte = min_inte; + } + + // Differential term + diff = -( 2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); + + // Output + out = prop + inte + diff; + if (out > max) { + out = max; + } else if (out < min) { + out = min; + } + + // Update memory + prev_err = err; + prev_measure = measure; + + requiredDrag = ; //equation with input of prev_measure (predicted altitude) to get output drag + return requiredDrag; + } } // END OF PID CONTROLLER CODE From 321888c4ed146375185a32c82b586c60367503ba Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Mon, 18 Jan 2021 00:14:22 -0500 Subject: [PATCH 09/23] Moved variables towards the top --- .../ORBrake/ORBrakeSimulationListener.java | 62 +++++++------------ 1 file changed, 24 insertions(+), 38 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index c5be8da..eeabbe7 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -11,6 +11,24 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { double velocity; double altitude; + + // Input parameters for PID controller + double Kp = 1; //proportional gain constant + double Ki = 1; //integral gain constant + double Kd = 1; //derivative gain constant + double tau = 1; //low pass filter time constant + double min = 1; //output min limit + double max = 5; //output max limit + double min_inte = 1; //integral min limit + double max_inte = 5; //integral max limit + double T = 1; //sample time in sec + + // Memory variables for PID controller + double inte; //integral term + double prev_err; //previous error + double diff; //differential term + double prev_measure; //previous measurement + double out; //output private static final double surfConst[][] = { // Surface constants for presimulated airbrake extensions. {-0.000000000, 0.000000000, -0.000000000, 0.0000000000, 0.000000000}, // 0 % @@ -62,40 +80,11 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust //return drag_coef * density * Math.pow(vel, 2) / 2; } - // PID CONTROLLER - - // Calculated parameters - - double err; //error - double SP; //setpoint (desired altitude) - double measure; //actual measurement (predicted altitude) - double requiredDrag; //required drag - - // Input parameters - - double Kp = 1; //proportional gain constant - double Ki = 1; //integral gain constant - double Kd = 1; //derivative gain constant - - double tau = 1; //low pass filter time constant - - double min = 1; //output min limit - double max = 5; //output max limit - double min_inte = 1; //integral min limit - double max_inte = 5; //integral max limit - - double T = 1; //sample time in sec - - // Memory variables - - double inte; //integral term - double prev_err; //previous error - double diff; //differential term - double prev_measure; //previous measurement - - double out; //output - - double PID_controller() + double requiredDrag(double SP, double measure) //PID controller to get updated drag coefficient + /** + * SP = desired altitude setpoint + * measure = actual measured predicted altitude + */ { // Initial conditions inte = 0; @@ -150,13 +139,10 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust prev_err = err; prev_measure = measure; - requiredDrag = ; //equation with input of prev_measure (predicted altitude) to get output drag - return requiredDrag; + return ; //required drag equation with input of prev_measure (predicted altitude) } } - // END OF PID CONTROLLER CODE - double extensionFromDrag(double requiredDrag) /** * Computes the required extension to achieve a required drag. From 88cc553dc616137fa988774dbb07736758575025 Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Mon, 18 Jan 2021 20:08:03 -0500 Subject: [PATCH 10/23] Updated PID controller function to integrate with rest of code --- .../ORBrake/ORBrakeSimulationListener.java | 78 +++++++++---------- 1 file changed, 37 insertions(+), 41 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 7cd9e76..91319d5 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -11,24 +11,23 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { double velocity; double altitude; + double thrust; + double setpoint; // Input parameters for PID controller double Kp = 1; //proportional gain constant double Ki = 1; //integral gain constant double Kd = 1; //derivative gain constant double tau = 1; //low pass filter time constant - double min = 1; //output min limit - double max = 5; //output max limit - double min_inte = 1; //integral min limit - double max_inte = 5; //integral max limit +// double min_inte = 1; //integral min limit +// double max_inte = 5; //integral max limit double T = 1; //sample time in sec // Memory variables for PID controller - double inte; //integral term - double prev_err; //previous error - double diff; //differential term - double prev_measure; //previous measurement - double out; //output + double inte = 0; //integral term + double prev_err = 0; //previous error + double diff = 0; //differential term + double prev_measure = 0; //previous measurement private static final double surfConst[][] = { // Surface constants for presimulated airbrake extensions. {-0.000000000, 0.000000000, -0.000000000, 0.0000000000, 0.000000000}, // 0 % @@ -70,12 +69,19 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust */ { // if (status.getSimulationTime() ) + this.thrust = thrust; + this.altitude = status.getRocketPosition().z; return thrust + airbrakeForce(); } double airbrakeForce() { - double requiredDrag = 3; + double requiredDrag = requiredDrag(setpoint, altitude); + if (requiredDrag > dragSurface(5)) { + requiredDrag = dragSurface(5); + } else if (requiredDrag < 0) { + requiredDrag = 0; + } return requiredDrag; //return drag_coef * density * Math.pow(vel, 2) / 2; } @@ -83,21 +89,14 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust double requiredDrag(double SP, double measure) //PID controller to get updated drag coefficient /** * SP = desired altitude setpoint - * measure = actual measured predicted altitude + * measure = actual altitude */ { - // Initial conditions - inte = 0; - prev_err = 0; - diff = 0; - prev_measure = 0; + // Initial conditions + double out = 0; - out = 0; - - while (SP - prev_measure > 0.01) - { - measure = out; - + if (thrust == 0) + { // Error function double err = SP - measure; @@ -105,19 +104,21 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust double prop = Kp*err; // Integral term - inte = 0.5*Ki*T*(err+prev_err) + inte; + inte += 0.5*Ki*T*(err+prev_err); - // Anti-wind up (static integral clamping) -// if (max > prop) { -// max_inte = max - prop; -// } else { -// max_inte = 0; -// } -// if (min < prop) { -// min_inte = min - prop; -// } else { -// min_inte = 0; -// } + // Anti-wind up (dynamic integral clamping) + double min_inte; //integral min limit + double max_inte; //integral max limit + if (dragSurface(5) > prop) { + max_inte = dragSurface(5) - prop; + } else { + max_inte = 0; + } + if (0 < prop) { + min_inte = 0 - prop; + } else { + min_inte = 0; + } if (inte > max_inte) { inte = max_inte; } else if (inte < min_inte) { @@ -129,18 +130,13 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust // Output out = prop + inte + diff; - if (out > max) { - out = max; - } else if (out < min) { - out = min; - } // Update memory prev_err = err; prev_measure = measure; - - return ; //required drag equation with input of prev_measure (predicted altitude) } + + return measure; //required drag } double extensionFromDrag(double requiredDrag) From b0e240681a9197c8dc15684d634616228097b9a3 Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Mon, 18 Jan 2021 20:11:08 -0500 Subject: [PATCH 11/23] Update return value of PID controller --- src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 91319d5..f793ea4 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -136,7 +136,7 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust prev_measure = measure; } - return measure; //required drag + return out; //required drag } double extensionFromDrag(double requiredDrag) From a4099000d0023cc79006ec1377a109e2d31df6c1 Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Mon, 18 Jan 2021 20:43:24 -0500 Subject: [PATCH 12/23] Commented out entire code, and added values for setpoint --- .../ORBrake/ORBrakeSimulationListener.java | 5 +- .../sf/openrocket/ORBrake/ORBrakeTest.java | 72 +++++++++---------- 2 files changed, 38 insertions(+), 39 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index f793ea4..3bbd3a8 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -12,7 +12,7 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { double velocity; double altitude; double thrust; - double setpoint; + double setpoint = 4550; //desired altitude in feet // Input parameters for PID controller double Kp = 1; //proportional gain constant @@ -82,8 +82,7 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust } else if (requiredDrag < 0) { requiredDrag = 0; } - return requiredDrag; - //return drag_coef * density * Math.pow(vel, 2) / 2; + return -requiredDrag; } double requiredDrag(double SP, double measure) //PID controller to get updated drag coefficient diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java index 9ce54c1..e2a89a2 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java @@ -1,38 +1,38 @@ -package net.sf.openrocket.ORBrake; - -import static org.junit.jupiter.api.Assertions.*; - -import org.junit.jupiter.api.AfterAll; -import org.junit.jupiter.api.AfterEach; -import org.junit.jupiter.api.BeforeAll; -import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Test; - -class ORBrakeTest { - - static ORBrakeSimulationListener listener; - - @BeforeAll - static void setUpBeforeClass() throws Exception { - listener = new ORBrakeSimulationListener(); - } - -// @AfterAll -// static void tearDownAfterClass() throws Exception { +//package net.sf.openrocket.ORBrake; +// +//import static org.junit.jupiter.api.Assertions.*; +// +//import org.junit.jupiter.api.AfterAll; +//import org.junit.jupiter.api.AfterEach; +//import org.junit.jupiter.api.BeforeAll; +//import org.junit.jupiter.api.BeforeEach; +//import org.junit.jupiter.api.Test; +// +//class ORBrakeTest { +// +// static ORBrakeSimulationListener listener; +// +// @BeforeAll +// static void setUpBeforeClass() throws Exception { +// listener = new ORBrakeSimulationListener(); // } - - @BeforeEach - void setUp() throws Exception { - listener.velocity = 300; - listener.altitude = 3000; - } - -// @AfterEach -// void tearDown() throws Exception { +// +//// @AfterAll +//// static void tearDownAfterClass() throws Exception { +//// } +// +// @BeforeEach +// void setUp() throws Exception { +// listener.velocity = 300; +// listener.altitude = 3000; // } - - @Test - void dragSurfaceTest() { - assertEquals(listener.dragSurface(5), 5555, .0001); - } -} +// +//// @AfterEach +//// void tearDown() throws Exception { +//// } +// +// @Test +// void dragSurfaceTest() { +// assertEquals(listener.dragSurface(5), 5555, .0001); +// } +//} From b28f67c22e741e74ef3d131956c41adb5ac59eda Mon Sep 17 00:00:00 2001 From: Max Schrader Date: Mon, 18 Jan 2021 21:48:28 -0500 Subject: [PATCH 13/23] Build file working with tests --- build.xml | 4 +- .../sf/openrocket/ORBrake/ORBrakeTest.java | 74 ++++++++++--------- 2 files changed, 41 insertions(+), 37 deletions(-) diff --git a/build.xml b/build.xml index 894be54..3935688 100644 --- a/build.xml +++ b/build.xml @@ -11,7 +11,9 @@ Building... - + + + Done diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java index e2a89a2..611278f 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java @@ -1,38 +1,40 @@ -//package net.sf.openrocket.ORBrake; -// -//import static org.junit.jupiter.api.Assertions.*; -// -//import org.junit.jupiter.api.AfterAll; -//import org.junit.jupiter.api.AfterEach; -//import org.junit.jupiter.api.BeforeAll; -//import org.junit.jupiter.api.BeforeEach; -//import org.junit.jupiter.api.Test; -// -//class ORBrakeTest { -// -// static ORBrakeSimulationListener listener; -// -// @BeforeAll -// static void setUpBeforeClass() throws Exception { -// listener = new ORBrakeSimulationListener(); +package net.sf.openrocket.ORBrake; + +import static org.junit.jupiter.api.Assertions.*; + +import org.junit.jupiter.api.AfterAll; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class ORBrakeTest { + + static ORBrakeSimulationListener listener; + + @BeforeAll + static void setUpBeforeClass() throws Exception { + listener = new ORBrakeSimulationListener(); + } + +// @AfterAll +// static void tearDownAfterClass() throws Exception { // } -// -//// @AfterAll -//// static void tearDownAfterClass() throws Exception { -//// } -// -// @BeforeEach -// void setUp() throws Exception { -// listener.velocity = 300; -// listener.altitude = 3000; + + @BeforeEach + void setUp() throws Exception { + listener.velocity = 300; + listener.altitude = 3000; + } + +// @AfterEach +// void tearDown() throws Exception { // } -// -//// @AfterEach -//// void tearDown() throws Exception { -//// } -// -// @Test -// void dragSurfaceTest() { -// assertEquals(listener.dragSurface(5), 5555, .0001); -// } -//} + + @Test + void dragSurfaceTest() { + double drag = listener.dragSurface(5); + System.out.println(drag); + assertEquals(drag, 4.8284, .0001); + } +} From 73aaa0495acf053cb79b85247313070407d1264b Mon Sep 17 00:00:00 2001 From: Max Schrader Date: Mon, 18 Jan 2021 22:40:47 -0500 Subject: [PATCH 14/23] Working controller --- .../ORBrake/ORBrakeSimulationListener.java | 120 +++++++++--------- .../sf/openrocket/ORBrake/ORBrakeTest.java | 6 +- 2 files changed, 65 insertions(+), 61 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 3bbd3a8..95b74be 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -9,15 +9,15 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { - double velocity; - double altitude; - double thrust; - double setpoint = 4550; //desired altitude in feet +// double velocity; +// double altitude; +// double thrust; + double setpoint = 4550/3.281; //desired altitude in feet // Input parameters for PID controller double Kp = 1; //proportional gain constant - double Ki = 1; //integral gain constant - double Kd = 1; //derivative gain constant + double Ki = 0; //integral gain constant + double Kd = 0; //derivative gain constant double tau = 1; //low pass filter time constant // double min_inte = 1; //integral min limit // double max_inte = 5; //integral max limit @@ -31,11 +31,11 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { private static final double surfConst[][] = { // Surface constants for presimulated airbrake extensions. {-0.000000000, 0.000000000, -0.000000000, 0.0000000000, 0.000000000}, // 0 % - {-0.023136854, 0.000010395, 0.000071445, -0.0000000862, 0.000008669}, // 20 % - {-0.041211681, 0.000017555, 0.000565077, -0.0000002520, 0.000017995}, // 40 % - {-0.085142430, 0.000038009, 0.000316569, -0.0000003705, 0.000029817}, // 60 % - {-0.152819731, 0.000068195, 0.000074012, -0.0000004884, 0.000043697}, // 80 % - {-0.261060050, 0.000115821, -0.000232887, -0.0000007869, 0.000061334} // 100% + {-0.102912726, -0.000151700, 0.001042665, 0.0000041259, 0.000415087}, // 20 % + {-0.183309557, -0.000256191, 0.008246675, 0.0000120688, 0.000861634}, // 40 % + {-0.378713528, -0.000554695, 0.004619966, 0.0000177406, 0.001427697}, // 60 % + {-0.679742164, -0.000995227, 0.001080119, 0.0000233844, 0.002092339}, // 80 % + {-1.161195104, -0.001690272, -0.003398721, 0.0000376809, 0.002936851} // 100% }; public ORBrakeSimulationListener() { @@ -53,7 +53,7 @@ public FlightConditions postFlightConditions(SimulationStatus status, FlightCond * @return null. */ { - velocity = conditions.getVelocity(); +// velocity = conditions.getVelocity(); return null; } @@ -69,23 +69,25 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust */ { // if (status.getSimulationTime() ) - this.thrust = thrust; - this.altitude = status.getRocketPosition().z; - return thrust + airbrakeForce(); +// status.getRocketVelocity().normalize(); +// this.thrust = thrust; +// this.altitude = status.getRocketPosition().z; + return thrust + airbrakeForce(status, thrust); } - double airbrakeForce() + double airbrakeForce(SimulationStatus status, double thrust) { - double requiredDrag = requiredDrag(setpoint, altitude); - if (requiredDrag > dragSurface(5)) { - requiredDrag = dragSurface(5); + double requiredDrag = requiredDrag(setpoint, status, thrust); + double surf = dragSurface(5, status.getRocketPosition().z, status.getRocketVelocity().length()); + if (requiredDrag > surf) { + requiredDrag = surf; } else if (requiredDrag < 0) { requiredDrag = 0; } return -requiredDrag; } - double requiredDrag(double SP, double measure) //PID controller to get updated drag coefficient + double requiredDrag(double SP,SimulationStatus status, double thrust) //PID controller to get updated drag coefficient /** * SP = desired altitude setpoint * measure = actual altitude @@ -93,6 +95,8 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust { // Initial conditions double out = 0; + double measure = status.getRocketPosition().z; + double velocity = status.getRocketVelocity().length(); if (thrust == 0) { @@ -108,8 +112,8 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust // Anti-wind up (dynamic integral clamping) double min_inte; //integral min limit double max_inte; //integral max limit - if (dragSurface(5) > prop) { - max_inte = dragSurface(5) - prop; + if (dragSurface(5, measure, velocity) > prop) { + max_inte = dragSurface(5, measure, velocity) - prop; } else { max_inte = 0; } @@ -138,43 +142,43 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust return out; //required drag } - double extensionFromDrag(double requiredDrag) - /** - * Computes the required extension to achieve a required drag. - * - * @param requiredDrag The desired drag from the control system. - * @return The percentage deployment that will produce that drag. - */ - { - double[] drag = new double[6]; - - // Compute drag for each known extension. - IntStream.range(0, 5).forEachOrdered(n -> { - drag[n] = this.dragSurface(n); - }); - - // Interpolate to find desired extension - double extension = 0; - double term; - - for (int i = 0; i < 5; ++i) - { - term = i; - for (int j = 0; j < 5; ++j) - { - if(j != i) - { - term *= (requiredDrag - drag[j]) / (drag[i] - drag[j]); - } - }; - - extension += term; - }; - extension *= 20; - return extension; - } +// double extensionFromDrag(double requiredDrag) +// /** +// * Computes the required extension to achieve a required drag. +// * +// * @param requiredDrag The desired drag from the control system. +// * @return The percentage deployment that will produce that drag. +// */ +// { +// double[] drag = new double[6]; +// +// // Compute drag for each known extension. +// IntStream.range(0, 5).forEachOrdered(n -> { +// drag[n] = this.dragSurface(n); +// }); +// +// // Interpolate to find desired extension +// double extension = 0; +// double term; +// +// for (int i = 0; i < 5; ++i) +// { +// term = i; +// for (int j = 0; j < 5; ++j) +// { +// if(j != i) +// { +// term *= (requiredDrag - drag[j]) / (drag[i] - drag[j]); +// } +// }; +// +// extension += term; +// }; +// extension *= 20; +// return extension; +// } - double dragSurface(int extNum) + double dragSurface(int extNum, double altitude, double velocity) /** * Finds the drag force at one of the 6 drag surfaces given the current * velocity and altitude. diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java index 611278f..4d859d8 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java @@ -23,8 +23,8 @@ static void setUpBeforeClass() throws Exception { @BeforeEach void setUp() throws Exception { - listener.velocity = 300; - listener.altitude = 3000; +// listener.velocity = 300; +// listener.altitude = 3000; } // @AfterEach @@ -33,7 +33,7 @@ void setUp() throws Exception { @Test void dragSurfaceTest() { - double drag = listener.dragSurface(5); + double drag = listener.dragSurface(5. 300. 3000); System.out.println(drag); assertEquals(drag, 4.8284, .0001); } From 55a42d0b418ad30c4c163966d0ea84dd62812c3f Mon Sep 17 00:00:00 2001 From: Max Schrader Date: Mon, 18 Jan 2021 22:41:36 -0500 Subject: [PATCH 15/23] syntax error --- src/net/sf/openrocket/ORBrake/ORBrakeTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java index 4d859d8..90e0c26 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java @@ -33,7 +33,7 @@ void setUp() throws Exception { @Test void dragSurfaceTest() { - double drag = listener.dragSurface(5. 300. 3000); + double drag = listener.dragSurface(5, 300, 3000); System.out.println(drag); assertEquals(drag, 4.8284, .0001); } From a365263ac242cecf7aab18523763bd15f1fcf2a4 Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Mon, 18 Jan 2021 23:04:15 -0500 Subject: [PATCH 16/23] Update ORBrakeSimulationListener.java --- src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 95b74be..ee479a3 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -129,7 +129,7 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust } // Differential term - diff = -( 2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); + diff = ( -2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); // Output out = prop + inte + diff; From 790a6ee115e621ba47cae6d212cf651373d43711 Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Mon, 18 Jan 2021 23:04:26 -0500 Subject: [PATCH 17/23] Revert "Update ORBrakeSimulationListener.java" This reverts commit a365263ac242cecf7aab18523763bd15f1fcf2a4. --- src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index ee479a3..95b74be 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -129,7 +129,7 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust } // Differential term - diff = ( -2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); + diff = -( 2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); // Output out = prop + inte + diff; From b82f629e77a839f5914e8ac8a4893c9ab0f1e2b8 Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Mon, 18 Jan 2021 23:05:01 -0500 Subject: [PATCH 18/23] Negative sign on Kd only --- src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 95b74be..ee479a3 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -129,7 +129,7 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust } // Differential term - diff = -( 2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); + diff = ( -2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); // Output out = prop + inte + diff; From a519a6d4b52c3d66801e693986a17ccec2aa64fd Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Tue, 19 Jan 2021 00:18:48 -0500 Subject: [PATCH 19/23] Added labels to panel --- src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java b/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java index 5216f32..b04bef4 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java @@ -23,7 +23,14 @@ public ORBrakeConfigurator() { @Override protected JComponent getConfigurationComponent(ORBrake extension, Simulation simulation, JPanel panel) { - // panel.add(new JLabel("Thrust multiplier:")); + panel.add(new JLabel("Setpoint:")); + + panel.add(new JLabel("Proportional Gain:")); + + panel.add(new JLabel("Integral Gain:")); + + panel.add(new JLabel("Differential Gain:")); + // DoubleModel m = new DoubleModel(extension, "Multiplier", UnitGroup.UNITS_RELATIVE, 0); From 1f7fa441615a49f577d0f296d9d39334ba2e822a Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Tue, 19 Jan 2021 00:35:37 -0500 Subject: [PATCH 20/23] Added slider and spinner per PID gain and setpoint --- .../ORBrake/ORBrakeConfigurator.java | 63 ++++++++++++++----- 1 file changed, 49 insertions(+), 14 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java b/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java index b04bef4..67e2ba6 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java @@ -23,26 +23,61 @@ public ORBrakeConfigurator() { @Override protected JComponent getConfigurationComponent(ORBrake extension, Simulation simulation, JPanel panel) { - panel.add(new JLabel("Setpoint:")); - - panel.add(new JLabel("Proportional Gain:")); + + panel.add(new JLabel("Setpoint:")); + DoubleModel S = new DoubleModel(extension, "setpoint", UnitGroup.UNITS_RELATIVE, 0); + + JSpinner spinS = new JSpinner(S.getSpinnerModel()); + spinS.setEditor(new SpinnerEditor(spinS)); + panel.add(spinS, "w 65lp!"); + + UnitSelector unitS = new UnitSelector(S); + panel.add(unitS, "w 25"); + + BasicSlider sliderS = new BasicSlider(S.getSliderModel(0, 100)); + panel.add(sliderS, "w 75lp, wrap"); + + + panel.add(new JLabel("Proportional Gain:")); + DoubleModel P = new DoubleModel(extension, "Kp", UnitGroup.UNITS_RELATIVE, 0); + + JSpinner spinP = new JSpinner(P.getSpinnerModel()); + spinP.setEditor(new SpinnerEditor(spinP)); + panel.add(spinP, "w 65lp!"); + + UnitSelector unitP = new UnitSelector(P); + panel.add(unitP, "w 25"); + + BasicSlider sliderP = new BasicSlider(P.getSliderModel(0, 10)); + panel.add(sliderP, "w 75lp, wrap"); + - panel.add(new JLabel("Integral Gain:")); + panel.add(new JLabel("Integral Gain:")); + DoubleModel I = new DoubleModel(extension, "Ki", UnitGroup.UNITS_RELATIVE, 0); + + JSpinner spinI = new JSpinner(I.getSpinnerModel()); + spinI.setEditor(new SpinnerEditor(spinI)); + panel.add(spinI, "w 65lp!"); + + UnitSelector unitI = new UnitSelector(I); + panel.add(unitI, "w 25"); + + BasicSlider sliderI = new BasicSlider(I.getSliderModel(0, 5)); + panel.add(sliderI, "w 75lp, wrap"); - panel.add(new JLabel("Differential Gain:")); - - // DoubleModel m = new DoubleModel(extension, "Multiplier", UnitGroup.UNITS_RELATIVE, 0); + panel.add(new JLabel("Differential Gain:")); + DoubleModel D = new DoubleModel(extension, "Kd", UnitGroup.UNITS_RELATIVE, 0); - // JSpinner spin = new JSpinner(m.getSpinnerModel()); - // spin.setEditor(new SpinnerEditor(spin)); - // panel.add(spin, "w 65lp!"); + JSpinner spinD = new JSpinner(D.getSpinnerModel()); + spinD.setEditor(new SpinnerEditor(spinD)); + panel.add(spinD, "w 65lp!"); - // UnitSelector unit = new UnitSelector(m); - // panel.add(unit, "w 25"); + UnitSelector unitD = new UnitSelector(D); + panel.add(unitD, "w 25"); - // BasicSlider slider = new BasicSlider(m.getSliderModel(0, 3)); - // panel.add(slider, "w 75lp, wrap"); + BasicSlider sliderD = new BasicSlider(D.getSliderModel(0, 3)); + panel.add(sliderD, "w 75lp, wrap"); return panel; } From 2290bea4e82d4f2c354ee97c88c0d95f8f510b3c Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Tue, 19 Jan 2021 00:38:30 -0500 Subject: [PATCH 21/23] Don't need units for gain constants --- src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java b/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java index 67e2ba6..48eb6ae 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java @@ -45,9 +45,6 @@ protected JComponent getConfigurationComponent(ORBrake extension, Simulation sim spinP.setEditor(new SpinnerEditor(spinP)); panel.add(spinP, "w 65lp!"); - UnitSelector unitP = new UnitSelector(P); - panel.add(unitP, "w 25"); - BasicSlider sliderP = new BasicSlider(P.getSliderModel(0, 10)); panel.add(sliderP, "w 75lp, wrap"); @@ -59,9 +56,6 @@ protected JComponent getConfigurationComponent(ORBrake extension, Simulation sim spinI.setEditor(new SpinnerEditor(spinI)); panel.add(spinI, "w 65lp!"); - UnitSelector unitI = new UnitSelector(I); - panel.add(unitI, "w 25"); - BasicSlider sliderI = new BasicSlider(I.getSliderModel(0, 5)); panel.add(sliderI, "w 75lp, wrap"); @@ -73,12 +67,10 @@ protected JComponent getConfigurationComponent(ORBrake extension, Simulation sim spinD.setEditor(new SpinnerEditor(spinD)); panel.add(spinD, "w 65lp!"); - UnitSelector unitD = new UnitSelector(D); - panel.add(unitD, "w 25"); - BasicSlider sliderD = new BasicSlider(D.getSliderModel(0, 3)); panel.add(sliderD, "w 75lp, wrap"); + return panel; } From d381926d5c2c3cfcca6533f333a7703f01eb52b8 Mon Sep 17 00:00:00 2001 From: nancynguyen1014 <74472825+nancynguyen1014@users.noreply.github.com> Date: Tue, 19 Jan 2021 23:24:28 -0500 Subject: [PATCH 22/23] Updated pop up window for ORBrake simulation --- src/net/sf/openrocket/ORBrake/ORBrake.java | 63 ++++++++++++++++++- .../ORBrake/ORBrakeConfigurator.java | 49 +++++++++++++-- .../ORBrake/ORBrakeSimulationListener.java | 20 +++--- 3 files changed, 118 insertions(+), 14 deletions(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrake.java b/src/net/sf/openrocket/ORBrake/ORBrake.java index bff9dd6..d5b04ed 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrake.java +++ b/src/net/sf/openrocket/ORBrake/ORBrake.java @@ -21,6 +21,67 @@ public String getDescription() @Override public void initialize(SimulationConditions conditions) throws SimulationException { - conditions.getSimulationListenerList().add(new ORBrakeSimulationListener()); + conditions.getSimulationListenerList().add(new ORBrakeSimulationListener(getSetpoint(), getKp(), getKi(), getKd(), getTau(), getT())); } + + public double getSetpoint() + { + return config.getDouble("setpoint", 100.0); + } + public void setSetpoint(double setpoint) + { + config.put("setpoint", setpoint); + fireChangeEvent(); + } + + public double getKp() + { + return config.getDouble("Kp", 5.0); + } + public void setKp(double Kp) + { + config.put("Kp", Kp); + fireChangeEvent(); + } + + public double getKi() + { + return config.getDouble("Ki", 0.0); + } + public void setKi(double Ki) + { + config.put("Ki", Ki); + fireChangeEvent(); + } + + public double getKd() + { + return config.getDouble("Kd", 0.0); + } + public void setKd(double Kd) + { + config.put("Kd", Kd); + fireChangeEvent(); + } + + public double getTau() + { + return config.getDouble("tau", 1.0); + } + public void setTau(double tau) + { + config.put("tau", tau); + fireChangeEvent(); + } + + public double getT() + { + return config.getDouble("T", 1.0); + } + public void setT(double T) + { + config.put("T", T); + fireChangeEvent(); + } + } \ No newline at end of file diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java b/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java index 48eb6ae..fe605d5 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java @@ -25,7 +25,7 @@ public ORBrakeConfigurator() { protected JComponent getConfigurationComponent(ORBrake extension, Simulation simulation, JPanel panel) { panel.add(new JLabel("Setpoint:")); - DoubleModel S = new DoubleModel(extension, "setpoint", UnitGroup.UNITS_RELATIVE, 0); + DoubleModel S = new DoubleModel(extension, "Setpoint", UnitGroup.UNITS_DISTANCE, 0); JSpinner spinS = new JSpinner(S.getSpinnerModel()); spinS.setEditor(new SpinnerEditor(spinS)); @@ -34,43 +34,80 @@ protected JComponent getConfigurationComponent(ORBrake extension, Simulation sim UnitSelector unitS = new UnitSelector(S); panel.add(unitS, "w 25"); - BasicSlider sliderS = new BasicSlider(S.getSliderModel(0, 100)); + BasicSlider sliderS = new BasicSlider(S.getSliderModel(0, 3100)); panel.add(sliderS, "w 75lp, wrap"); panel.add(new JLabel("Proportional Gain:")); - DoubleModel P = new DoubleModel(extension, "Kp", UnitGroup.UNITS_RELATIVE, 0); + DoubleModel P = new DoubleModel(extension, "Kp", UnitGroup.UNITS_COEFFICIENT, 0); JSpinner spinP = new JSpinner(P.getSpinnerModel()); spinP.setEditor(new SpinnerEditor(spinP)); panel.add(spinP, "w 65lp!"); + UnitSelector unitP = new UnitSelector(P); + panel.add(unitP, "w 25"); + BasicSlider sliderP = new BasicSlider(P.getSliderModel(0, 10)); panel.add(sliderP, "w 75lp, wrap"); panel.add(new JLabel("Integral Gain:")); - DoubleModel I = new DoubleModel(extension, "Ki", UnitGroup.UNITS_RELATIVE, 0); + DoubleModel I = new DoubleModel(extension, "Ki", UnitGroup.UNITS_COEFFICIENT, 0); JSpinner spinI = new JSpinner(I.getSpinnerModel()); spinI.setEditor(new SpinnerEditor(spinI)); panel.add(spinI, "w 65lp!"); + UnitSelector unitI = new UnitSelector(I); + panel.add(unitI, "w 25"); + BasicSlider sliderI = new BasicSlider(I.getSliderModel(0, 5)); panel.add(sliderI, "w 75lp, wrap"); panel.add(new JLabel("Differential Gain:")); - DoubleModel D = new DoubleModel(extension, "Kd", UnitGroup.UNITS_RELATIVE, 0); + DoubleModel D = new DoubleModel(extension, "Kd", UnitGroup.UNITS_COEFFICIENT, 0); JSpinner spinD = new JSpinner(D.getSpinnerModel()); spinD.setEditor(new SpinnerEditor(spinD)); panel.add(spinD, "w 65lp!"); - BasicSlider sliderD = new BasicSlider(D.getSliderModel(0, 3)); + UnitSelector unitD = new UnitSelector(D); + panel.add(unitD, "w 25"); + + BasicSlider sliderD = new BasicSlider(D.getSliderModel(0, 5)); panel.add(sliderD, "w 75lp, wrap"); + panel.add(new JLabel("Time Constant:")); + DoubleModel Tau = new DoubleModel(extension, "Tau", UnitGroup.UNITS_COEFFICIENT, 0); + + JSpinner spinTau = new JSpinner(Tau.getSpinnerModel()); + spinTau.setEditor(new SpinnerEditor(spinTau)); + panel.add(spinTau, "w 65lp!"); + + UnitSelector unitTau = new UnitSelector(Tau); + panel.add(unitTau, "w 25"); + + BasicSlider sliderTau = new BasicSlider(Tau.getSliderModel(0, 3)); + panel.add(sliderTau, "w 75lp, wrap"); + + + panel.add(new JLabel("Sample Time:")); + DoubleModel T = new DoubleModel(extension, "T", UnitGroup.UNITS_TIME_STEP, 0); + + JSpinner spinT = new JSpinner(T.getSpinnerModel()); + spinT.setEditor(new SpinnerEditor(spinT)); + panel.add(spinT, "w 65lp!"); + + UnitSelector unitT = new UnitSelector(T); + panel.add(unitT, "w 25"); + + BasicSlider sliderT = new BasicSlider(T.getSliderModel(0, 3)); + panel.add(sliderT, "w 75lp, wrap"); + + return panel; } diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index ee479a3..523a04c 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java @@ -12,16 +12,16 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { // double velocity; // double altitude; // double thrust; - double setpoint = 4550/3.281; //desired altitude in feet + double setpoint; //desired altitude in feet // Input parameters for PID controller - double Kp = 1; //proportional gain constant - double Ki = 0; //integral gain constant - double Kd = 0; //derivative gain constant - double tau = 1; //low pass filter time constant + double Kp; //proportional gain constant + double Ki; //integral gain constant + double Kd; //derivative gain constant + double tau; //low pass filter time constant // double min_inte = 1; //integral min limit // double max_inte = 5; //integral max limit - double T = 1; //sample time in sec + double T; //sample time in sec // Memory variables for PID controller double inte = 0; //integral term @@ -38,8 +38,14 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { {-1.161195104, -0.001690272, -0.003398721, 0.0000376809, 0.002936851} // 100% }; - public ORBrakeSimulationListener() { + public ORBrakeSimulationListener(double setpoint, double Kp, double Ki, double Kd, double tau, double T) { super(); + this.setpoint = setpoint; + this.Kp = Kp; + this.Ki = Ki; + this.Kd = Kd; + this.tau = tau; + this.T = T; } @Override From e328afb8f9ae8c1266aa2c34a6485c2d1814ded2 Mon Sep 17 00:00:00 2001 From: Max Schrader Date: Tue, 19 Jan 2021 23:45:23 -0500 Subject: [PATCH 23/23] Updated test cases for new constructor --- src/net/sf/openrocket/ORBrake/ORBrakeTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java index 90e0c26..56a3e2d 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java @@ -14,7 +14,7 @@ class ORBrakeTest { @BeforeAll static void setUpBeforeClass() throws Exception { - listener = new ORBrakeSimulationListener(); + listener = new ORBrakeSimulationListener(2000.0, 10.0, 0.0, 0.0, 1.0, .5); } // @AfterAll