MotorStateFeedback.java

/*
 * Created on 2011/07/20
 * Copyright (C) 2011 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.model.matxbook.composite;


import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.tool.control.model.matxbook.MotorWithFullState;
import org.mklab.tool.control.model.matxbook.StateFeedback;
import org.mklab.tool.control.system.DoubleSystemBuilder;
import org.mklab.tool.control.system.DoubleSystemOperator;
import org.mklab.tool.control.system.source.DoubleConstantSource;


/**
 * モーターの状態フィードバック制御システムを生成するクラスです。
 * 
 * @author koga
 * @version $Revision$, 2011/07/20
 */
public class MotorStateFeedback {

  private DoubleSystemBuilder motor;
  private DoubleSystemBuilder reference;
  private DoubleSystemBuilder stateFeedback;

  /**
   * モーターの状態フィードバック制御システムを返します。
   * 
   * @return モーターの状態フィードバック制御システム
   */
  public DoubleSystemOperator getSystem() {
    createPlant();
    createController();
    createAuxiliary();

    DoubleSystemBuilder openLoop = this.motor.multiply(this.stateFeedback);
    DoubleSystemBuilder closedLoop = openLoop.unityFeedback();
    DoubleSystemBuilder all = closedLoop.multiply(this.reference);
    return all.getSystemOperator();
  }

  private void createPlant() {
    // モーター
    MotorWithFullState motorSystem = new MotorWithFullState();
    DoubleMatrix initialState = new DoubleMatrix(new double[][] { {1}, {0}});
    motorSystem.setInitialState(initialState);
    this.motor = new DoubleSystemBuilder(motorSystem);
  }

  private void createController() {
    // 状態フィードバック
    StateFeedback stateFeedbackSystem = new StateFeedback();
    this.stateFeedback = new DoubleSystemBuilder(stateFeedbackSystem);
  }

  private void createAuxiliary() {
    // 零入力
    DoubleMatrix refernceState = new DoubleMatrix(new double[] {0, 0}).transpose();
    DoubleConstantSource referenceSystem = new DoubleConstantSource(refernceState);
    this.reference = new DoubleSystemBuilder(referenceSystem);
  }
}