ContinuousPIDSystem.java

/*
 * Created on 2012/07/10
 * Copyright (C) 2012 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.system.continuous;

import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.tool.control.LinearSystemFactory;


/**
 * 連続時間PIDシステムを表すクラスです。
 * 
 * @author esumi
 * @version $Revision$, 2012/07/23
 * @param <RS> type of real scalar
 * @param <RM> type of real matrix
 * @param <CS> type of complex scalar
 * @param <CM> type of complex matrix
 */
public class ContinuousPIDSystem<RS extends RealNumericalScalar<RS,RM,CS,CM>, RM extends RealNumericalMatrix<RS,RM,CS,CM>, CS extends ComplexNumericalScalar<RS,RM,CS,CM>, CM extends ComplexNumericalMatrix<RS,RM,CS,CM>> extends ContinuousLinearDynamicSystem<RS,RM,CS,CM> {

  /** 連続PIDコントローラのタイプ */
  private ContinuousPIDType pidType = ContinuousPIDType.PARALELL;
  
  /**
   * Creates {@link ContinuousPIDSystem}.
   * @param sunit unit of scalar
   */
  public ContinuousPIDSystem(RS sunit) {
    super(sunit);
  }

  /**
   * 連続PIDコントローラのタイプを設定します。
   * 
   * @param pidType 連続PIDコントローラのタイプ
   */
  public void setPidType(ContinuousPIDType pidType) {
    this.pidType = pidType;
  }

  /**
   * PIDシステムを設定します。
   * 
   * @param p 比例項P
   * @param i 積分項I
   * @param d 微分項D
   * @param n フィルターN
   */
  public void setLinearSystem(RS p, RS i, RS d, RS n) {
    RM A;
    RM B;
    RM C;
    RM D;
    
    RS[][] aa = this.sunit.createArray(2, 2);
    RS[][] bb = this.sunit.createArray(2, 1);    
    RS[][] cc = this.sunit.createArray(1, 2);
    RS[][] dd = this.sunit.createArray(1, 1);
    
    switch (this.pidType) {
      case PARALELL:
        aa[0][0] = this.sunit.createZero();
        aa[0][1] = this.sunit.createZero();
        aa[1][1] = this.sunit.createZero();
        aa[1][1] = n.unaryMinus();
        A = this.sunit.createGrid(aa);
        
        bb[0][0] = i;
        bb[1][0] =n.multiply(d);
        B = this.sunit.createGrid(bb);

        cc[0][0] = this.sunit.createUnit();
        cc[0][1] = n.unaryMinus();
        C = this.sunit.createGrid(cc);

        dd[0][0] = p.add(n.multiply(d));
        D = this.sunit.createGrid(dd);
        break;
      case IDEAL:
        aa[0][0] = this.sunit.createZero();
        aa[0][1] = this.sunit.createZero();
        aa[1][1] = this.sunit.createZero();
        aa[1][1] = n.unaryMinus();
        A = this.sunit.createGrid(aa);

        bb[0][0] = i;
        bb[1][0] =n.multiply(d);
        B = this.sunit.createGrid(bb);
        
        cc[0][0] = p;
        cc[0][1] = p.multiply(n).unaryMinus();
        C = this.sunit.createGrid(cc);

        dd[0][0] = p.add(p.multiply(n).multiply(d));
        D = this.sunit.createGrid(dd);
        break;
      default:
        throw new IllegalArgumentException();
    }
    setLinearSystem(LinearSystemFactory.createLinearSystem(A, B, C, D));
  }

}