git.gag.com Git - debian/openrocket/blob - src/net/sf/openrocket/simulation/listeners/haisu/RollControlListener.java

   1 package net.sf.openrocket.simulation.listeners.haisu;
   2
   3 import net.sf.openrocket.aerodynamics.FlightConditions;
   4 import net.sf.openrocket.rocketcomponent.FinSet;
   5 import net.sf.openrocket.rocketcomponent.RocketComponent;
   6 import net.sf.openrocket.simulation.SimulationStatus;
   7 import net.sf.openrocket.simulation.listeners.AbstractSimulationListener;
   8 import net.sf.openrocket.util.MathUtil;
   9
  10 /**
  11  * An example listener that applies a PI-controller to adjust the cant of fins
  12  * named "CONTROL" to stop the rocket from rolling.
  13  *
  14  * @author Sampo Niskanen <sampo.niskanen@iki.fi>
  15  */
  16 public class RollControlListener extends AbstractSimulationListener {
  17
  18         private static final double DELTA_T = 0.01;
  19         private static final double START_TIME = 0.5;
  20
  21         private static final double SETPOINT = 0.0;
  22
  23         private static final double TURNRATE = 10 * Math.PI / 180; // per second
  24
  25
  26         /*
  27          * At M=0.3 KP oscillation threshold between 0.35 and 0.4.    Good KI=3
  28          * At M=0.6 KP oscillation threshold between 0.07 and 0.08    Good KI=2
  29          * At M=0.9 KP oscillation threshold between 0.013 and 0.014  Good KI=0.5
  30          */
  31         private static final double KP = 0.007;
  32         private static final double KI = 0.2;
  33
  34
  35         private static final double MAX_ANGLE = 15 * Math.PI / 180;
  36
  37
  38
  39
  40         private double rollrate;
  41
  42         private double intState = 0;
  43
  44         private double finPosition = 0;
  45
  46
  47         public RollControlListener() {
  48         }
  49
  50         @Override
  51         public FlightConditions postFlightConditions(SimulationStatus status, FlightConditions flightConditions) {
  52                 rollrate = flightConditions.getRollRate();
  53                 return null;
  54         }
  55
  56         @Override
  57         public void postStep(SimulationStatus status) {
  58
  59                 if (status.getSimulationTime() < START_TIME)
  60                         return;
  61
  62                 // PID controller
  63                 FinSet finset = null;
  64                 for (RocketComponent c : status.getConfiguration()) {
  65                         if ((c instanceof FinSet) && (c.getName().equals("CONTROL"))) {
  66                                 finset = (FinSet) c;
  67                                 break;
  68                         }
  69                 }
  70                 if (finset == null) {
  71                         throw new RuntimeException("CONTROL fin not found");
  72                 }
  73
  74
  75                 double error = SETPOINT - rollrate;
  76
  77
  78                 error = Math.signum(error) * error * error; ////  pow2(error)
  79
  80                 double p = KP * error;
  81                 intState += error * DELTA_T;
  82                 double i = KI * intState;
  83
  84                 double value = p + i;
  85
  86
  87                 if (Math.abs(value) > MAX_ANGLE) {
  88                         System.err.printf("Attempting to set angle %.1f at t=%.3f, clamping.\n",
  89                                         value * 180 / Math.PI, status.getSimulationTime());
  90                         value = MathUtil.clamp(value, -MAX_ANGLE, MAX_ANGLE);
  91                 }
  92
  93
  94                 if (finPosition < value) {
  95                         finPosition = Math.min(finPosition + TURNRATE * DELTA_T, value);
  96                 } else {
  97                         finPosition = Math.max(finPosition - TURNRATE * DELTA_T, value);
  98                 }
  99
 100                 if (MathUtil.equals(status.getSimulationTime() * 10, Math.rint(status.getSimulationTime() * 10))) {
 101                         System.err.printf("t=%.3f  angle=%.1f  current=%.1f\n", status.getSimulationTime(),
 102                                         value * 180 / Math.PI, finPosition * 180 / Math.PI);
 103                 }
 104
 105                 finset.setCantAngle(finPosition);
 106
 107         }
 108
 109 }