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/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 5216f32..fe605d5 100644 --- a/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java +++ b/src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java @@ -23,19 +23,90 @@ public ORBrakeConfigurator() { @Override protected JComponent getConfigurationComponent(ORBrake extension, Simulation simulation, JPanel panel) { - // panel.add(new JLabel("Thrust multiplier:")); - // DoubleModel m = new DoubleModel(extension, "Multiplier", UnitGroup.UNITS_RELATIVE, 0); + panel.add(new JLabel("Setpoint:")); + DoubleModel S = new DoubleModel(extension, "Setpoint", UnitGroup.UNITS_DISTANCE, 0); - // JSpinner spin = new JSpinner(m.getSpinnerModel()); - // spin.setEditor(new SpinnerEditor(spin)); - // panel.add(spin, "w 65lp!"); + JSpinner spinS = new JSpinner(S.getSpinnerModel()); + spinS.setEditor(new SpinnerEditor(spinS)); + panel.add(spinS, "w 65lp!"); - // UnitSelector unit = new UnitSelector(m); - // panel.add(unit, "w 25"); + UnitSelector unitS = new UnitSelector(S); + panel.add(unitS, "w 25"); + + 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_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_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_COEFFICIENT, 0); + + JSpinner spinD = new JSpinner(D.getSpinnerModel()); + 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, 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"); - // BasicSlider slider = new BasicSlider(m.getSliderModel(0, 3)); - // panel.add(slider, "w 75lp, wrap"); return panel; } diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java b/src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java index 316d0ac..523a04c 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; @@ -8,31 +9,193 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener { - private double velocity; - private double drag_coef = 1.3; - private double density = 1.225; +// double velocity; +// double altitude; +// double thrust; + double setpoint; //desired altitude in feet + + // Input parameters for PID controller + 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; //sample time in sec + + // Memory variables for PID controller + 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 % + {-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() { + 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 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(); +// 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); +// status.getRocketVelocity().normalize(); +// this.thrust = thrust; +// this.altitude = status.getRocketPosition().z; + return thrust + airbrakeForce(status, thrust); + } + + double airbrakeForce(SimulationStatus status, double thrust) + { + 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; } - private double AirbrakeForce(double vel) + double requiredDrag(double SP,SimulationStatus status, double thrust) //PID controller to get updated drag coefficient + /** + * SP = desired altitude setpoint + * measure = actual altitude + */ + { + // Initial conditions + double out = 0; + double measure = status.getRocketPosition().z; + double velocity = status.getRocketVelocity().length(); + + if (thrust == 0) + { + // Error function + double err = SP - measure; + + // Proportional term + double prop = Kp*err; + + // Integral term + inte += 0.5*Ki*T*(err+prev_err); + + // Anti-wind up (dynamic integral clamping) + double min_inte; //integral min limit + double max_inte; //integral max limit + if (dragSurface(5, measure, velocity) > prop) { + max_inte = dragSurface(5, measure, velocity) - 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) { + inte = min_inte; + } + + // Differential term + diff = ( -2*Kd*(measure-prev_measure) + (2*tau-T)*diff ) / (2*tau+T); + + // Output + out = prop + inte + diff; + + // Update memory + prev_err = err; + prev_measure = measure; + } + + 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 dragSurface(int extNum, double altitude, double velocity) + /** + * 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 diff --git a/src/net/sf/openrocket/ORBrake/ORBrakeTest.java b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java new file mode 100644 index 0000000..56a3e2d --- /dev/null +++ b/src/net/sf/openrocket/ORBrake/ORBrakeTest.java @@ -0,0 +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(2000.0, 10.0, 0.0, 0.0, 1.0, .5); + } + +// @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() { + double drag = listener.dragSurface(5, 300, 3000); + System.out.println(drag); + assertEquals(drag, 4.8284, .0001); + } +}