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 java.util.Collection;
   4
   5 import net.sf.openrocket.aerodynamics.FlightConditions;
   6 import net.sf.openrocket.rocketcomponent.FinSet;
   7 import net.sf.openrocket.rocketcomponent.RocketComponent;
   8 import net.sf.openrocket.simulation.FlightEvent;
   9 import net.sf.openrocket.simulation.SimulationStatus;
  10 import net.sf.openrocket.simulation.listeners.AbstractSimulationListener;
  11 import net.sf.openrocket.util.MathUtil;
  12
  13
  14 public class RollControlListener extends AbstractSimulationListener {
  15
  16         private static final double DELTA_T = 0.01;
  17         private static final double START_TIME = 0.5;
  18
  19         private static final double MACH = 0.9;
  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 void flightConditions(SimulationStatus status, FlightConditions conditions) {
  52                 // Limit movement:
  53
  54 //              conditions.setAOA(0);
  55 //              conditions.setTheta(0);
  56 //              conditions.setMach(MACH);
  57 //              conditions.setPitchRate(0);
  58 //              conditions.setYawRate(0);
  59 //              status.position = new Coordinate(0,0,100);
  60 //              status.velocity = Coordinate.NUL;
  61
  62
  63                 rollrate = conditions.getRollRate();
  64         }
  65
  66         @Override
  67         public Collection<FlightEvent> stepTaken(SimulationStatus status) {
  68
  69                 if (status.time < START_TIME)
  70                         return null;
  71
  72                 // PID controller
  73                 FinSet finset = null;
  74                 for (RocketComponent c: status.configuration) {
  75                         if ((c instanceof FinSet) && (c.getName().equals("CONTROL"))) {
  76                                 finset = (FinSet)c;
  77                                 break;
  78                         }
  79                 }
  80                 if (finset==null) {
  81                         throw new RuntimeException("CONTROL fin not found");
  82                 }
  83
  84
  85                 double error = SETPOINT - rollrate;
  86
  87
  88                 error = Math.signum(error) * error * error;    ////  pow2(error)
  89
  90                 double p = KP * error;
  91                 intState += error * DELTA_T;
  92                 double i = KI * intState;
  93
  94                 double value = p+i;
  95
  96
  97                 if (Math.abs(value) > MAX_ANGLE) {
  98                         System.err.printf("Attempting to set angle %.1f at t=%.3f, clamping.\n",
  99                                         value*180/Math.PI, status.time);
 100                         value = MathUtil.clamp(value, -MAX_ANGLE, MAX_ANGLE);
 101                 }
 102
 103
 104                 if (finPosition < value) {
 105                         finPosition = Math.min(finPosition + TURNRATE*DELTA_T, value);
 106                 } else {
 107                         finPosition = Math.max(finPosition - TURNRATE*DELTA_T, value);
 108                 }
 109
 110                 if (MathUtil.equals(status.time*10, Math.rint(status.time*10))) {
 111                         System.err.printf("t=%.3f  angle=%.1f  current=%.1f\n",status.time,
 112                                         value*180/Math.PI, finPosition*180/Math.PI);
 113                 }
 114
 115                 finset.setCantAngle(finPosition);
 116
 117                 return null;
 118         }
 119
 120
 121
 122
 123
 124 }