Merge pull request #2 from WPI-HPRC/Development

Development

Author: cm-schrader

src/net/sf/openrocket/ORBrake/ORBrake.java

@@ -6,6 +6,9 @@
 
 public class ORBrake extends AbstractSimulationExtension 
 {
+	/**
+	 * GUI back end and listener instantiation.
+	 */
     @Override
     public String getName() 
     {
@@ -21,7 +24,7 @@ public String getDescription()
     @Override
     public void initialize(SimulationConditions conditions) throws SimulationException
     {
-        conditions.getSimulationListenerList().add(new ORBrakeSimulationListener(getSetpoint(), getKp(), getKi(), getKd(), getTau(), getT()));
+        conditions.getSimulationListenerList().add(new ORBrakeSimulationListener(getSetpoint(), getKp(), getKi(), getKd(), getTau(), getCd(), getMass()));
     }
 
     public double getSetpoint()
@@ -74,13 +77,24 @@ public void setTau(double tau)
     	fireChangeEvent();
     }
 
-    public double getT()
+    public double getCd()
     {
-    	return config.getDouble("T", 1.0);
+    	return config.getDouble("Cd", .49);
     }
-    public void setT(double T)
+    public void setCd(double Cd)
     {
-    	config.put("T", T);
+    	config.put("Cd", Cd);
+    	fireChangeEvent();
+    }
+    
+    
+    public double getMass()
+    {
+    	return config.getDouble("Mass", 20.02);
+    }
+    public void setMass(double mass)
+    {
+    	config.put("Mass", mass);
     	fireChangeEvent();
     }
 

src/net/sf/openrocket/ORBrake/ORBrakeConfigurator.java

@@ -16,6 +16,9 @@
 
 @Plugin
 public class ORBrakeConfigurator extends AbstractSwingSimulationExtensionConfigurator<ORBrake> {
+	/**
+	 * The configurator is responsible for creating the config GUI when the extension is loaded.
+	 */
 
 	public ORBrakeConfigurator() {
 		super(ORBrake.class);
@@ -94,20 +97,33 @@ protected JComponent getConfigurationComponent(ORBrake extension, Simulation sim
 		panel.add(sliderTau, "w 75lp, wrap");
 
 
-		panel.add(new JLabel("Sample Time:"));
-		DoubleModel T = new DoubleModel(extension, "T", UnitGroup.UNITS_TIME_STEP, 0);
+		panel.add(new JLabel("Drag Coeficient:"));
+		DoubleModel Cd = new DoubleModel(extension, "Cd", UnitGroup.UNITS_COEFFICIENT, 0);
 
-		JSpinner spinT = new JSpinner(T.getSpinnerModel());
-		spinT.setEditor(new SpinnerEditor(spinT));
-		panel.add(spinT, "w 65lp!");
+		JSpinner spinCd = new JSpinner(Cd.getSpinnerModel());
+		spinCd.setEditor(new SpinnerEditor(spinCd));
+		panel.add(spinCd, "w 65lp!");
 
-		UnitSelector unitT = new UnitSelector(T);
-		panel.add(unitT, "w 25");
+		UnitSelector unitCd = new UnitSelector(Cd);
+		panel.add(unitCd, "w 25");
 
-		BasicSlider sliderT = new BasicSlider(T.getSliderModel(0, 3));
-		panel.add(sliderT, "w 75lp, wrap");
+		BasicSlider sliderCd = new BasicSlider(Cd.getSliderModel(0, 2));
+		panel.add(sliderCd, "w 75lp, wrap");
 
 
+		panel.add(new JLabel("Estimate Mass:"));
+		DoubleModel Mass = new DoubleModel(extension, "Mass", UnitGroup.UNITS_COEFFICIENT, 0);
+		
+		JSpinner spinMass = new JSpinner(Mass.getSpinnerModel());
+		spinMass.setEditor(new SpinnerEditor(spinMass));
+		panel.add(spinMass, "w 65lp!");
+		
+		UnitSelector unitMass = new UnitSelector(Mass);
+		panel.add(unitMass, "w 25");
+		
+		BasicSlider sliderMass = new BasicSlider(Mass.getSliderModel(0, 40));
+		panel.add(sliderMass, "w 75lp, wrap");
+		
 		return panel;
 	}
 

src/net/sf/openrocket/ORBrake/ORBrakeProvider.java

@@ -7,9 +7,12 @@
 
 @Plugin
 public class ORBrakeProvider extends AbstractSimulationExtensionProvider {
+	/**
+	 * Lists the plugin in the simulation options.
+	 */
 
 	public ORBrakeProvider() {
-		super(ORBrake.class, "Flight", "OR Brake");
+		super(ORBrake.class, "WPI", "OR Brake");
 	}
 
 }

src/net/sf/openrocket/ORBrake/ORBrakeSimulationListener.java

@@ -8,26 +8,29 @@
 import net.sf.openrocket.aerodynamics.FlightConditions;
 
 public class ORBrakeSimulationListener extends AbstractSimulationListener {
+	/**
+	 * The simulation listener connects to and influences simulations that is
+	 * attached to.  
+	 */
 
-//    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
+	double setpoint; // Target altitude in feet
+	double Kp; 			// Proportional gain constant
+    double Ki; 			// Integral gain constant
+    double Kd; 			// Derivative gain constant
+    double tau; 		// Low pass filter time constant
+    double T = .05; 	// Sample time in sec
+    
+    // Input parameters for apogee estimator
+    double Cd;
+    double mass;
 
     // 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
+    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  %
@@ -38,30 +41,28 @@ public class ORBrakeSimulationListener extends AbstractSimulationListener {
     		{-1.161195104, -0.001690272, -0.003398721, 0.0000376809, 0.002936851}	// 100%
     };
 
-	public ORBrakeSimulationListener(double setpoint, double Kp, double Ki, double Kd, double tau, double T) {
+	public ORBrakeSimulationListener(double setpoint, double Kp, double Ki, double Kd, double tau, double Cd, double mass) {
 		super();
 		this.setpoint = setpoint;
 		this.Kp = Kp;
 		this.Ki = Ki;
 		this.Kd = Kd;
 		this.tau = tau;
-		this.T = T;
+		this.Cd = Cd;
+		this.mass = mass;
 	}
 
 	@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; 
-    }
+	public void startSimulation(SimulationStatus status)
+	/**
+	 * Gets the time step at the start of the simulation.
+	 * 
+	 * @param	status The status object at the start of the sim.
+	 * @return void
+	 */
+	{
+    	T = status.getSimulationConditions().getTimeStep();
+	}
 
     @Override
     public double postSimpleThrustCalculation(SimulationStatus status, double thrust) // throws SimulationException
@@ -74,16 +75,13 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust
      * @return	The modified thrust to be actually applied.
      */
     {
-        // if (status.getSimulationTime() )
-//    	status.getRocketVelocity().normalize();
-//    	this.thrust = thrust;
-//    	this.altitude = status.getRocketPosition().z;
-        return thrust + airbrakeForce(status, thrust);
+    	double drag = airbrakeForce(status, thrust);
+        return thrust + drag;
     }
 
     double airbrakeForce(SimulationStatus status, double thrust)
     {
-    	double requiredDrag = requiredDrag(setpoint, status, thrust);
+    	double requiredDrag = requiredDrag(status, thrust);
     	double surf = dragSurface(5, status.getRocketPosition().z, status.getRocketVelocity().length());
     	if (requiredDrag > surf) {
     		requiredDrag = surf;
@@ -93,33 +91,45 @@ public double postSimpleThrustCalculation(SimulationStatus status, double thrust
         return -requiredDrag;
     }
 
-    double requiredDrag(double SP,SimulationStatus status, double thrust) //PID controller to get updated drag coefficient
+    double requiredDrag(SimulationStatus status, double thrust) //PID controller to get updated drag coefficient
     /**
-     * SP = desired altitude setpoint
-     * measure = actual altitude
+     * Computes required drag using a PID controller.
+     * 
+     * @param	status	The current simulation status object.
+     * @param	thrust	The current thrust of the vehicle.
      */
     {
     	// Initial conditions  	
     	double out = 0;
-    	double measure = status.getRocketPosition().z;
+    	double alt = status.getRocketPosition().z;
     	double velocity = status.getRocketVelocity().length();
+    	double vertVelocity = status.getRocketVelocity().z;
 
+//    	double mass = status.getSimulationConditions().getRocket().getMass();
+//    	double Cd = status.getSimulationConditions().getAerodynamicCalculator().getAerodynamicForces().getCD();
+    	double gravity = status.getSimulationConditions().getGravityModel().getGravity(status.getRocketWorldPosition());
+    	double refArea = status.getConfiguration().getReferenceArea();
+    	
+    	double termVelocity = Math.sqrt((2*mass*gravity)/( Cd * refArea *1.225));
+    	double predApogee = alt+(((Math.pow(termVelocity,2))/(2*gravity))* Math.log((Math.pow(vertVelocity,2)+Math.pow(termVelocity,2))/(Math.pow(termVelocity,2))));
+
+    	// PID Controller
     	if (thrust == 0)
     	{    		
     		// Error function
-	    	double err = SP - measure;
+	    	double err = setpoint - predApogee;
 
 	    	// Proportional term
-	    	double prop = Kp*err;
+	    	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;
+	    	if (dragSurface(5, predApogee, velocity) > prop) {
+	    		max_inte = dragSurface(5, predApogee, velocity) - prop;
 	    	} else {
 	    		max_inte = 0;
 	    	}
@@ -135,17 +145,17 @@ 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*(predApogee - prev_measure) + (2*tau-T)*diff ) / (2*tau+T);
 
 	    	// Output 
 	    	out = prop + inte + diff;
 
 	    	// Update memory
 	    	prev_err = err;
-	    	prev_measure = measure;
+	    	prev_measure = predApogee;
 	    }
 
-    	return out; //required drag
+    	return out; 
     }
 
 //    double extensionFromDrag(double requiredDrag)

src/net/sf/openrocket/ORBrake/ORBrakeTest.java

@@ -14,7 +14,7 @@ class ORBrakeTest {
 
 	@BeforeAll
 	static void setUpBeforeClass() throws Exception {
-		listener = new ORBrakeSimulationListener(2000.0, 10.0, 0.0, 0.0, 1.0, .5);
+		listener = new ORBrakeSimulationListener(2000.0, 10.0, 0.0, 0.0, 1.0, 10, .4);
 	}
 
 //	@AfterAll