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(1, 1, 2, 1);
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(1, 1);
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(1, 2);
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 }
|