Motor2DiscreteObserver.java
001 /*
002  * $Id$
003  *
004  * Copyright (C) 2004 Koga Laboratory. All rights reserved.
005  *
006  */
007 package matxbook.chap21;
008 
009 import org.mklab.nfc.matrix.Matrix;
010 import org.mklab.tool.control.system.discrete.ZeroOrderHoldSystem;
011 import org.mklab.tool.control.system.sampled.BaseSampledDataDynamicSystem;
012 
013 
014 /**
015  * モータと離散時間オブザーバーの結合システムを表すクラスです。
016  
017  @author koga
018  @version $Revision$, 2004/04/23
019  */
020 public class Motor2DiscreteObserver extends BaseSampledDataDynamicSystem {
021   /** モータ */
022   Motor motor = new Motor2();
023   /** 状態フィードバック制御器 */
024   StateFeedback stateFeedback = new StateFeedback();
025   /** 離散時間オブザーバー */
026   DiscreteObserver discObserver = new DiscreteObserver();
027   /** ゼロ次ホールド */
028   ZeroOrderHoldSystem hold = new ZeroOrderHoldSystem();
029 
030   /**
031    * コンストラクター
032    */
033   public Motor2DiscreteObserver() {
034     super(1121);
035     setForcedSystem(true);
036   }
037 
038   /**
039    @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#continuousStateEquation(double, org.mklab.nfc.matrix.Matrix, org.mklab.nfc.matrix.Matrix, org.mklab.nfc.matrix.Matrix)
040    */
041   public Matrix continuousStateEquation(double t, Matrix xc, @SuppressWarnings("unused"Matrix xd, Matrix inputOutput) {
042     this.motor.setState(xc);
043 
044     Matrix u = inputOutput.getSubVector(11);
045     return this.motor.stateEquation(t, this.motor.getState(), u);
046   }
047 
048   /**
049    @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#discreteStateEquation(double, org.mklab.nfc.matrix.Matrix, org.mklab.nfc.matrix.Matrix, org.mklab.nfc.matrix.Matrix)
050    */
051   public Matrix discreteStateEquation(double t, Matrix xc, Matrix xd, Matrix inputOutput) {
052     this.motor.setState(xc);
053     this.discObserver.setState(xd);
054 
055     Matrix uy = inputOutput.getSubVector(12);
056     int k = (int)(t / this.discObserver.getSamplingInterval());
057     return this.discObserver.stateEquation(k, this.discObserver.getState(), uy);
058   }
059 
060   /**
061    @see org.mklab.nfc.ode.DifferentialDifferenceSystem#inputOutputEquation(double, org.mklab.nfc.matrix.Matrix, org.mklab.nfc.matrix.Matrix)
062    */
063   public Matrix inputOutputEquation(double t, Matrix xc, Matrix xd) {
064     this.motor.setState(xc);
065     this.discObserver.setState(xd);
066 
067     Matrix y = this.motor.outputEquation(t, this.motor.getState());
068 
069     Matrix xh;
070     if (this.hold.isAtSamplingPoint()) {
071       int k = (int)Math.floor(t / this.discObserver.getSamplingInterval());
072       xh = this.discObserver.outputEquation(k, this.discObserver.getState(), y);
073       xh = this.hold.outputEquation(t, xh);
074     else {
075       xh = this.hold.outputEquation(t, null);
076     }
077 
078     Matrix u = this.stateFeedback.outputEquation(t, xh);
079     return u.appendDown(y);
080   }
081 
082   /**
083    @see org.mklab.nfc.ode.DifferentialDifferenceSystem#getNextSamplingTime(double, double)
084    */
085   public double getNextSamplingTime(double t, double tolerance) {
086     double samplingInterval = this.discObserver.getSamplingInterval();
087     double nextSamplingTime = Math.ceil(t / samplingInterval* samplingInterval;
088     if (Math.abs(t - nextSamplingTime< tolerance) {
089       nextSamplingTime = t + this.discObserver.getSamplingInterval();
090     }
091     return nextSamplingTime;
092   }
093 
094   /**
095    @see org.mklab.nfc.ode.DifferentialDifferenceSystem#setAtSamplingPoint(boolean)
096    */
097   @Override
098   public void setAtSamplingPoint(boolean samplingPoint) {
099     super.setAtSamplingPoint(samplingPoint);
100     this.hold.setAtSamplingPoint(samplingPoint);
101     this.discObserver.setAtSamplingPoint(samplingPoint);
102   }
103 
104   /**
105    @see org.mklab.nfc.ode.DifferentialDifferenceSystem#isAtSamplingPoint()
106    */
107   @Override
108   public boolean isAtSamplingPoint() {
109     return this.hold.isAtSamplingPoint() || this.discObserver.isAtSamplingPoint();
110   }
111 
112   /**
113    * サンプリング周期を設定します。
114    
115    @param interval サンプリング周期
116    */
117   public void setSamplingInterval(double interval) {
118     this.hold.setSamplingInterval(interval);
119     this.discObserver.setSamplingInterval(interval);
120   }
121 
122   /**
123    @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#setContinuousInitialState(org.mklab.nfc.matrix.Matrix)
124    */
125   @Override
126   public void setContinuousInitialState(Matrix initialState) {
127     this.motor.setInitialState(initialState);
128   }
129 
130   /**
131    @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#setDiscreteInitialState(org.mklab.nfc.matrix.Matrix)
132    */
133   @Override
134   public void setDiscreteInitialState(Matrix initialState) {
135     this.discObserver.setInitialState(initialState);
136   }
137 
138   /**
139    @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getContinuousInitialState()
140    */
141   @Override
142   public Matrix getContinuousInitialState() {
143     return this.motor.getInitialState();
144   }
145 
146   /**
147    @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getDiscreteInitialState()
148    */
149   @Override
150   public Matrix getDiscreteInitialState() {
151     return this.discObserver.getInitialState();
152   }
153 
154   /**
155    @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#setContinuousState(org.mklab.nfc.matrix.Matrix)
156    */
157   @Override
158   public void setContinuousState(Matrix state) {
159     this.motor.setState(state);
160   }
161 
162   /**
163    @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#setDiscreteState(org.mklab.nfc.matrix.Matrix)
164    */
165   @Override
166   public void setDiscreteState(Matrix state) {
167     this.discObserver.setState(state);
168   }
169 
170   /**
171    @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getContinuousState()
172    */
173   @Override
174   public Matrix getContinuousState() {
175     return this.motor.getState();
176   }
177 
178   /**
179    @see org.mklab.tool.control.system.sampled.SampledDataDynamicSystem#getDiscreteState()
180    */
181   @Override
182   public Matrix getDiscreteState() {
183     return this.discObserver.getState();
184   }
185 
186 }