Convert Roll control example listener to simulation extension
This commit is contained in:
JoePfeiffer
parent
f427cad09f
commit
9027b21a12
@ -0,0 +1,206 @@
|
|||||||
|
package net.sf.openrocket.simulation.extension.impl;
|
||||||
|
|
||||||
|
import java.util.ArrayList;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
import net.sf.openrocket.aerodynamics.FlightConditions;
|
||||||
|
import net.sf.openrocket.rocketcomponent.FinSet;
|
||||||
|
import net.sf.openrocket.rocketcomponent.RocketComponent;
|
||||||
|
import net.sf.openrocket.simulation.FlightDataType;
|
||||||
|
import net.sf.openrocket.simulation.SimulationConditions;
|
||||||
|
import net.sf.openrocket.simulation.SimulationStatus;
|
||||||
|
import net.sf.openrocket.simulation.exception.SimulationException;
|
||||||
|
import net.sf.openrocket.simulation.extension.AbstractSimulationExtension;
|
||||||
|
import net.sf.openrocket.simulation.listeners.AbstractSimulationListener;
|
||||||
|
import net.sf.openrocket.unit.UnitGroup;
|
||||||
|
import net.sf.openrocket.util.MathUtil;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* An example listener that applies a PI-controller to adjust the cant of fins
|
||||||
|
* to control the rocket's roll rate
|
||||||
|
*
|
||||||
|
* @author Sampo Niskanen <sampo.niskanen@iki.fi>
|
||||||
|
*
|
||||||
|
* One note: making aerodynamic changes during the simulation results in the simulation
|
||||||
|
* running *extremely* slowly
|
||||||
|
*/
|
||||||
|
public class RollControl extends AbstractSimulationExtension {
|
||||||
|
|
||||||
|
// save fin cant angle as a FlightDataType
|
||||||
|
private static final ArrayList<FlightDataType> types = new ArrayList<FlightDataType>();
|
||||||
|
private static final FlightDataType FIN_CANT_TYPE = FlightDataType.getType("Control fin cant", "\u03B1fc", UnitGroup.UNITS_ANGLE);
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void initialize(SimulationConditions conditions) throws SimulationException {
|
||||||
|
conditions.getSimulationListenerList().add(new RollControlListener());
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public String getName() {
|
||||||
|
return "Roll Control";
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public String getDescription() {
|
||||||
|
return "Use a PID control to control a rocket's roll. The current cant angle of the control finset is published to flight data as \u03B1fc. "
|
||||||
|
+ "Since this extension modifies design parameters during the simulation, it causes the simulation to run <b>much</b> more slowly.";
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public List<FlightDataType> getFlightDataTypes() {
|
||||||
|
return types;
|
||||||
|
}
|
||||||
|
|
||||||
|
RollControl() {
|
||||||
|
types.add(FIN_CANT_TYPE);
|
||||||
|
}
|
||||||
|
|
||||||
|
public String getControlFinName() {
|
||||||
|
return config.getString("controlFinName", "CONTROL");
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setControlFinName(String name) {
|
||||||
|
config.put("controlFinName", name);
|
||||||
|
fireChangeEvent();
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getStartTime() {
|
||||||
|
return config.getDouble("startTime", 0.5);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setStartTime(double startTime) {
|
||||||
|
config.put("startTime", startTime);
|
||||||
|
fireChangeEvent();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Desired roll rate (rad/sec)
|
||||||
|
public double getSetPoint() {
|
||||||
|
return config.getDouble("setPoint", 0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setSetPoint(double rollRate) {
|
||||||
|
config.put("setPoint", rollRate);
|
||||||
|
fireChangeEvent();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Maximum control fin turn rate (rad/sec)
|
||||||
|
public double getFinRate() {
|
||||||
|
return config.getDouble("finRate", 10 * Math.PI/180);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setFinRate(double finRate) {
|
||||||
|
config.put("finRate", finRate);
|
||||||
|
fireChangeEvent();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Maximum control fin angle (rad)
|
||||||
|
public double getMaxFinAngle() {
|
||||||
|
return config.getDouble("maxFinAngle", 15 * Math.PI / 180);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setMaxFinAngle(double maxFin) {
|
||||||
|
config.put("maxFinAngle", maxFin);
|
||||||
|
fireChangeEvent();
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getKP() {
|
||||||
|
return config.getDouble("KP", 0.007);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setKP(double KP) {
|
||||||
|
config.put("KP", KP);
|
||||||
|
fireChangeEvent();
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getKI() {
|
||||||
|
return config.getDouble("KI", 0.2);
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setKI(double KI) {
|
||||||
|
config.put("KI", KI);
|
||||||
|
fireChangeEvent();
|
||||||
|
}
|
||||||
|
|
||||||
|
private class RollControlListener extends AbstractSimulationListener {
|
||||||
|
private FinSet finset;
|
||||||
|
|
||||||
|
private double rollRate;
|
||||||
|
|
||||||
|
private double prevTime = 0;
|
||||||
|
private double intState = 0;
|
||||||
|
|
||||||
|
private double initialFinPosition;
|
||||||
|
private double finPosition = 0;
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void startSimulation(SimulationStatus status) throws SimulationException {
|
||||||
|
|
||||||
|
// Find the fin set
|
||||||
|
finset = null;
|
||||||
|
for (RocketComponent c : status.getConfiguration().getActiveComponents()) {
|
||||||
|
if ((c instanceof FinSet) && c.getName().equals(getControlFinName())) {
|
||||||
|
finset = (FinSet) c;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (finset == null) {
|
||||||
|
throw new SimulationException("A fin set with name '" + getControlFinName() + "' was not found");
|
||||||
|
}
|
||||||
|
|
||||||
|
// remember the initial fin position so we can set it back after running the simulation
|
||||||
|
initialFinPosition = finset.getCantAngle();
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public FlightConditions postFlightConditions(SimulationStatus status, FlightConditions flightConditions) {
|
||||||
|
// Store the current roll rate for later use
|
||||||
|
rollRate = flightConditions.getRollRate();
|
||||||
|
return null;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void postStep(SimulationStatus status) throws SimulationException {
|
||||||
|
// Activate PID controller only after a specific time
|
||||||
|
if (status.getSimulationTime() < getStartTime()) {
|
||||||
|
prevTime = status.getSimulationTime();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Determine time step
|
||||||
|
double deltaT = status.getSimulationTime() - prevTime;
|
||||||
|
prevTime = status.getSimulationTime();
|
||||||
|
|
||||||
|
// PID controller
|
||||||
|
double error = getSetPoint() - rollRate;
|
||||||
|
|
||||||
|
double p = getKP() * error;
|
||||||
|
intState += error * deltaT;
|
||||||
|
double i = getKI() * intState;
|
||||||
|
|
||||||
|
double value = p + i;
|
||||||
|
|
||||||
|
// Limit the fin turn rate
|
||||||
|
if (finPosition < value) {
|
||||||
|
finPosition = Math.min(finPosition + getFinRate() * deltaT, value);
|
||||||
|
} else {
|
||||||
|
finPosition = Math.max(finPosition - getFinRate() * deltaT, value);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clamp the fin angle between bounds
|
||||||
|
if (Math.abs(value) > getMaxFinAngle()) {
|
||||||
|
System.err.printf("Attempting to set angle %.1f at t=%.3f, clamping.\n",
|
||||||
|
value * 180 / Math.PI, status.getSimulationTime());
|
||||||
|
value = MathUtil.clamp(value, -getMaxFinAngle(), getMaxFinAngle());
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the control fin cant and store the data
|
||||||
|
finset.setCantAngle(finPosition);
|
||||||
|
status.getFlightData().setValue(FIN_CANT_TYPE, finPosition);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void endSimulation(SimulationStatus status, SimulationException exception) {
|
||||||
|
finset.setCantAngle(initialFinPosition);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@ -0,0 +1,13 @@
|
|||||||
|
package net.sf.openrocket.simulation.extension.impl;
|
||||||
|
|
||||||
|
import net.sf.openrocket.plugin.Plugin;
|
||||||
|
import net.sf.openrocket.simulation.extension.AbstractSimulationExtensionProvider;
|
||||||
|
|
||||||
|
@Plugin
|
||||||
|
public class RollControlProvider extends AbstractSimulationExtensionProvider {
|
||||||
|
|
||||||
|
public RollControlProvider() {
|
||||||
|
super(RollControl.class, "Control Enhancements", "Roll Control");
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@ -1,114 +0,0 @@
|
|||||||
package net.sf.openrocket.simulation.listeners.example;
|
|
||||||
|
|
||||||
import net.sf.openrocket.aerodynamics.FlightConditions;
|
|
||||||
import net.sf.openrocket.rocketcomponent.FinSet;
|
|
||||||
import net.sf.openrocket.rocketcomponent.RocketComponent;
|
|
||||||
import net.sf.openrocket.simulation.FlightDataType;
|
|
||||||
import net.sf.openrocket.simulation.SimulationStatus;
|
|
||||||
import net.sf.openrocket.simulation.exception.SimulationException;
|
|
||||||
import net.sf.openrocket.simulation.listeners.AbstractSimulationListener;
|
|
||||||
import net.sf.openrocket.unit.UnitGroup;
|
|
||||||
import net.sf.openrocket.util.MathUtil;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* An example listener that applies a PI-controller to adjust the cant of fins
|
|
||||||
* named "CONTROL" to stop the rocket from rolling.
|
|
||||||
*
|
|
||||||
* @author Sampo Niskanen <sampo.niskanen@iki.fi>
|
|
||||||
*/
|
|
||||||
public class RollControlListener extends AbstractSimulationListener {
|
|
||||||
|
|
||||||
// Name of control fin set
|
|
||||||
private static final String CONTROL_FIN_NAME = "CONTROL";
|
|
||||||
|
|
||||||
// Define custom flight data type
|
|
||||||
private static final FlightDataType FIN_CANT_TYPE = FlightDataType.getType("Control fin cant", "\u03B1fc", UnitGroup.UNITS_ANGLE);
|
|
||||||
|
|
||||||
// Simulation time at which PID controller is activated
|
|
||||||
private static final double START_TIME = 0.5;
|
|
||||||
|
|
||||||
// Desired roll rate (rad/sec)
|
|
||||||
private static final double SETPOINT = 0.0;
|
|
||||||
|
|
||||||
// Maximum control fin turn rate (rad/sec)
|
|
||||||
private static final double TURNRATE = 10 * Math.PI / 180;
|
|
||||||
|
|
||||||
// Maximum control fin angle (rad)
|
|
||||||
private static final double MAX_ANGLE = 15 * Math.PI / 180;
|
|
||||||
|
|
||||||
/*
|
|
||||||
* PID parameters
|
|
||||||
*
|
|
||||||
* At M=0.3 KP oscillation threshold between 0.35 and 0.4. Good KI=3
|
|
||||||
* At M=0.6 KP oscillation threshold between 0.07 and 0.08 Good KI=2
|
|
||||||
* At M=0.9 KP oscillation threshold between 0.013 and 0.014 Good KI=0.5
|
|
||||||
*/
|
|
||||||
private static final double KP = 0.007;
|
|
||||||
private static final double KI = 0.2;
|
|
||||||
|
|
||||||
private double rollRate;
|
|
||||||
|
|
||||||
private double prevTime = 0;
|
|
||||||
private double intState = 0;
|
|
||||||
|
|
||||||
private double finPosition = 0;
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public FlightConditions postFlightConditions(SimulationStatus status, FlightConditions flightConditions) {
|
|
||||||
// Store the current roll rate for later use
|
|
||||||
rollRate = flightConditions.getRollRate();
|
|
||||||
return null;
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
|
||||||
public void postStep(SimulationStatus status) throws SimulationException {
|
|
||||||
// Activate PID controller only after a specific time
|
|
||||||
if (status.getSimulationTime() < START_TIME) {
|
|
||||||
prevTime = status.getSimulationTime();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Find the fin set named CONTROL
|
|
||||||
FinSet finset = null;
|
|
||||||
for (RocketComponent c : status.getConfiguration().getActiveComponents()) {
|
|
||||||
if ((c instanceof FinSet) && (c.getName().equals(CONTROL_FIN_NAME))) {
|
|
||||||
finset = (FinSet) c;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (finset == null) {
|
|
||||||
throw new SimulationException("A fin set with name '" + CONTROL_FIN_NAME + "' was not found");
|
|
||||||
}
|
|
||||||
|
|
||||||
// Determine time step
|
|
||||||
double deltaT = status.getSimulationTime() - prevTime;
|
|
||||||
prevTime = status.getSimulationTime();
|
|
||||||
|
|
||||||
// PID controller
|
|
||||||
double error = SETPOINT - rollRate;
|
|
||||||
|
|
||||||
double p = KP * error;
|
|
||||||
intState += error * deltaT;
|
|
||||||
double i = KI * intState;
|
|
||||||
|
|
||||||
double value = p + i;
|
|
||||||
|
|
||||||
// Clamp the fin angle between -MAX_ANGLE and MAX_ANGLE
|
|
||||||
if (Math.abs(value) > MAX_ANGLE) {
|
|
||||||
System.err.printf("Attempting to set angle %.1f at t=%.3f, clamping.\n",
|
|
||||||
value * 180 / Math.PI, status.getSimulationTime());
|
|
||||||
value = MathUtil.clamp(value, -MAX_ANGLE, MAX_ANGLE);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Limit the fin turn rate
|
|
||||||
if (finPosition < value) {
|
|
||||||
finPosition = Math.min(finPosition + TURNRATE * deltaT, value);
|
|
||||||
} else {
|
|
||||||
finPosition = Math.max(finPosition - TURNRATE * deltaT, value);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Set the control fin cant and store the data
|
|
||||||
finset.setCantAngle(finPosition);
|
|
||||||
status.getFlightData().setValue(FIN_CANT_TYPE, finPosition);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Binary file not shown.
BIN
swing/resources/datafiles/examples/Simulation Extension.ork
Normal file
BIN
swing/resources/datafiles/examples/Simulation Extension.ork
Normal file
Binary file not shown.
@ -0,0 +1,83 @@
|
|||||||
|
package net.sf.openrocket.simulation.extension.impl;
|
||||||
|
|
||||||
|
import javax.swing.JComponent;
|
||||||
|
import javax.swing.JLabel;
|
||||||
|
import javax.swing.JPanel;
|
||||||
|
import javax.swing.JSpinner;
|
||||||
|
import javax.swing.JTextField;
|
||||||
|
import java.awt.event.FocusEvent;
|
||||||
|
import java.awt.event.FocusListener;
|
||||||
|
|
||||||
|
|
||||||
|
import net.sf.openrocket.document.Simulation;
|
||||||
|
import net.sf.openrocket.gui.SpinnerEditor;
|
||||||
|
import net.sf.openrocket.gui.adaptors.DoubleModel;
|
||||||
|
import net.sf.openrocket.gui.components.BasicSlider;
|
||||||
|
import net.sf.openrocket.gui.components.UnitSelector;
|
||||||
|
import net.sf.openrocket.plugin.Plugin;
|
||||||
|
import net.sf.openrocket.simulation.extension.AbstractSwingSimulationExtensionConfigurator;
|
||||||
|
import net.sf.openrocket.unit.UnitGroup;
|
||||||
|
|
||||||
|
@Plugin
|
||||||
|
public class RollControlConfigurator extends AbstractSwingSimulationExtensionConfigurator<RollControl> {
|
||||||
|
private JPanel panel;
|
||||||
|
private RollControl extension;
|
||||||
|
|
||||||
|
public RollControlConfigurator() {
|
||||||
|
super(RollControl.class);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
protected JComponent getConfigurationComponent(RollControl extension, Simulation simulation, JPanel panel) {
|
||||||
|
this.panel = panel;
|
||||||
|
this.extension = extension;
|
||||||
|
panel.add(new JLabel("Control FinSet Name:"));
|
||||||
|
|
||||||
|
JTextField finSetName = new JTextField();
|
||||||
|
finSetName.setText(extension.getControlFinName());
|
||||||
|
|
||||||
|
finSetName.addFocusListener(new FocusListener() {
|
||||||
|
@Override
|
||||||
|
public void focusGained(FocusEvent e) {}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void focusLost(FocusEvent e) {
|
||||||
|
extension.setControlFinName(finSetName.getText());
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
panel.add(finSetName, "growx, span3, wrap");
|
||||||
|
|
||||||
|
addRow("Start Time", "StartTime", UnitGroup.UNITS_SHORT_TIME, 0.0, 1200.0);
|
||||||
|
addRow("Desired Roll Rate", "SetPoint", UnitGroup.UNITS_ROLL, -10.0, 10.0);
|
||||||
|
addRow("Max Fin Turn Rate", "FinRate", UnitGroup.UNITS_ROLL, 0, 10);
|
||||||
|
addRow("Max Fin Angle", "MaxFinAngle", UnitGroup.UNITS_ANGLE, 0, 0.25);
|
||||||
|
|
||||||
|
panel.add(new JLabel("PID parameters:"), "newline 0.25in, span4, wrap");
|
||||||
|
panel.add(new JLabel("At M=0.3 KP oscillation threshold between 0.35 and 0.4. Good KI=3" ), "gapbefore indent, span4, wrap");
|
||||||
|
panel.add(new JLabel("At M=0.6 KP oscillation threshold between 0.07 and 0.08 Good KI=2" ), "gapbefore indent, span4, wrap");
|
||||||
|
panel.add(new JLabel("At M=0.9 KP oscillation threshold between 0.013 and 0.014 Good KI=0.5"), "gapbefore indent, span4, wrap");
|
||||||
|
|
||||||
|
addRow("KP (Proportional)", "KP", UnitGroup.UNITS_COEFFICIENT, 0.0, 0.02);
|
||||||
|
addRow("KI (Integrated)", "KI", UnitGroup.UNITS_COEFFICIENT, 0.0, 1.0);
|
||||||
|
|
||||||
|
return panel;
|
||||||
|
}
|
||||||
|
|
||||||
|
private void addRow(String prompt, String fieldName, UnitGroup units, double min, double max) {
|
||||||
|
panel.add(new JLabel(prompt + ":"));
|
||||||
|
|
||||||
|
DoubleModel m = new DoubleModel(extension, fieldName, units, min, max);
|
||||||
|
|
||||||
|
JSpinner spin = new JSpinner(m.getSpinnerModel());
|
||||||
|
spin.setEditor(new SpinnerEditor(spin));
|
||||||
|
panel.add(spin, "w 65lp!");
|
||||||
|
|
||||||
|
UnitSelector unit = new UnitSelector(m);
|
||||||
|
panel.add(unit, "w 25");
|
||||||
|
|
||||||
|
BasicSlider slider = new BasicSlider(m.getSliderModel(0, 1000));
|
||||||
|
panel.add(slider, "w 75lp, wrap");
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
Loading…
x
Reference in New Issue
Block a user