DoubleDiscretePIDSystem.java

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

import org.mklab.nfc.scalar.DoublePolynomial;
import org.mklab.nfc.scalar.DoubleRationalPolynomial;
import org.mklab.tool.control.DoubleLinearSystemFactory;


/**
 * 離散時間PIDシステムを表すクラスです。
 * 
 * @author esumi
 * @version $Revision$, 2012/07/24
 */
public class DoubleDiscretePIDSystem extends DoubleDiscreteLinearDynamicSystem {

  /** PIDコントローラのタイプ */
  private DiscretePIDType pidType = DiscretePIDType.PARALELL;
  /** 積分手法 */
  private DiscretePIDIntegralType integralPattern = DiscretePIDIntegralType.FORWARD_EULER;
  /** フィルター手法 */
  private DiscretePIDIntegralType filterPattern = DiscretePIDIntegralType.FORWARD_EULER;

  /**
   * PIDシステムを設定します。
   * 
   * @param p 比例項P
   * @param i 積分項I
   * @param d 微分項D
   * @param n フィルターN
   */
  public void setLinearSystem(double p, double i, double d, double n) {

    DoublePolynomial aNum = null;
    DoublePolynomial aDen = null;
    DoublePolynomial bNum = null;
    DoublePolynomial bDen = null;

    double smTime = getSamplingInterval();

    switch (this.integralPattern) {
      case FORWARD_EULER:
        aNum = new DoublePolynomial(new double[] {smTime});
        aDen = new DoublePolynomial(new double[] {-1, 1});
        break;
      case BACKWARD_EULER:
        aNum = new DoublePolynomial(new double[] {0, smTime});
        aDen = new DoublePolynomial(new double[] {-1, 1});
        break;
      case TRAPEZOIDAL:
        aNum = new DoublePolynomial(new double[] {smTime, smTime});
        aDen = new DoublePolynomial(new double[] {-2, 2});
        break;
      default:
        throw new IllegalArgumentException();
    }

    switch (this.filterPattern) {
      case FORWARD_EULER:
        bNum = new DoublePolynomial(new double[] {smTime});
        bDen = new DoublePolynomial(new double[] {-1, 1});
        break;
      case BACKWARD_EULER:
        bNum = new DoublePolynomial(new double[] {0, smTime});
        bDen = new DoublePolynomial(new double[] {-1, 1});
        break;
      case TRAPEZOIDAL:
        bNum = new DoublePolynomial(new double[] {smTime, smTime});
        bDen = new DoublePolynomial(new double[] {-2, 2});
        break;
      default:
        throw new IllegalArgumentException();
    }

    DoubleRationalPolynomial a = new DoubleRationalPolynomial(aNum, aDen);
    DoubleRationalPolynomial b = new DoubleRationalPolynomial(bNum, bDen);

    DoubleRationalPolynomial Gs;

    switch (this.pidType) {
      case PARALELL:
        Gs = a.multiply(i).add(p).add(b.multiply(n).add(1).inverse().multiply(n).multiply(d));
        break;
      case IDEAL:
        Gs = a.multiply(i).add(1).add(b.multiply(n).add(1).inverse().multiply(n).multiply(d)).multiply(p);
        break;
      default:
        throw new IllegalArgumentException();
    }
    setLinearSystem(DoubleLinearSystemFactory.createLinearSystem(Gs));
  }

  /**
   * 積分手法を設定します。
   * 
   * @param integralPattern 積分手法
   */
  public void setIntegralPattern(DiscretePIDIntegralType integralPattern) {
    this.integralPattern = integralPattern;
  }

  /**
   * フィルター手法を設定します。
   * 
   * @param filterPattern フィルター手法
   */
  public void setFilterPattern(DiscretePIDIntegralType filterPattern) {
    this.filterPattern = filterPattern;
  }

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