From 8888b73e160520e5b15e168998013f4a5f6e64c0 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 6 Jan 2013 15:41:23 -0500 Subject: Added control library. --- apps/systemlib/control/blocks.cpp | 486 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 486 insertions(+) create mode 100644 apps/systemlib/control/blocks.cpp (limited to 'apps/systemlib/control/blocks.cpp') diff --git a/apps/systemlib/control/blocks.cpp b/apps/systemlib/control/blocks.cpp new file mode 100644 index 000000000..c6c374300 --- /dev/null +++ b/apps/systemlib/control/blocks.cpp @@ -0,0 +1,486 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file blocks.cpp + * + * Controller library code + */ + +#include +#include + +#include "blocks.hpp" + +namespace control +{ + +int basicBlocksTest() +{ + blockLimitTest(); + blockLimitSymTest(); + blockLowPassTest(); + blockHighPassTest(); + blockIntegralTest(); + blockIntegralTrapTest(); + blockDerivativeTest(); + blockPTest(); + blockPITest(); + blockPDTest(); + blockPIDTest(); + blockOutputTest(); + blockRandUniformTest(); + blockRandGaussTest(); + return 0; +} + +float BlockLimit::update(float input) +{ + if (input > getMax()) { + input = _max.get(); + + } else if (input < getMin()) { + input = getMin(); + } + + return input; +} + +int blockLimitTest() +{ + printf("Test BlockLimit\t\t\t: "); + BlockLimit limit(NULL, "TEST"); + // initial state + ASSERT(equal(1.0f, limit.getMax())); + ASSERT(equal(-1.0f, limit.getMin())); + ASSERT(equal(0.0f, limit.getDt())); + // update + ASSERT(equal(-1.0f, limit.update(-2.0f))); + ASSERT(equal(1.0f, limit.update(2.0f))); + ASSERT(equal(0.0f, limit.update(0.0f))); + printf("PASS\n"); + return 0; +} + +float BlockLimitSym::update(float input) +{ + if (input > getMax()) { + input = _max.get(); + + } else if (input < -getMax()) { + input = -getMax(); + } + + return input; +} + +int blockLimitSymTest() +{ + printf("Test BlockLimitSym\t\t: "); + BlockLimitSym limit(NULL, "TEST"); + // initial state + ASSERT(equal(1.0f, limit.getMax())); + ASSERT(equal(0.0f, limit.getDt())); + // update + ASSERT(equal(-1.0f, limit.update(-2.0f))); + ASSERT(equal(1.0f, limit.update(2.0f))); + ASSERT(equal(0.0f, limit.update(0.0f))); + printf("PASS\n"); + return 0; +} + +float BlockLowPass::update(float input) +{ + float b = 2 * float(M_PI) * getFCut() * getDt(); + float a = b / (1 + b); + setState(a * input + (1 - a)*getState()); + return getState(); +} + +int blockLowPassTest() +{ + printf("Test BlockLowPass\t\t: "); + BlockLowPass lowPass(NULL, "TEST_LP"); + // test initial state + ASSERT(equal(10.0f, lowPass.getFCut())); + ASSERT(equal(0.0f, lowPass.getState())); + ASSERT(equal(0.0f, lowPass.getDt())); + // set dt + lowPass.setDt(0.1f); + ASSERT(equal(0.1f, lowPass.getDt())); + // set state + lowPass.setState(1.0f); + ASSERT(equal(1.0f, lowPass.getState())); + // test update + ASSERT(equal(1.8626974f, lowPass.update(2.0f))); + + // test end condition + for (int i = 0; i < 100; i++) { + lowPass.update(2.0f); + } + + ASSERT(equal(2.0f, lowPass.getState())); + ASSERT(equal(2.0f, lowPass.update(2.0f))); + printf("PASS\n"); + return 0; +}; + +float BlockHighPass::update(float input) +{ + float b = 2 * float(M_PI) * getFCut() * getDt(); + float a = 1 / (1 + b); + setY(a * (getY() + input - getU())); + setU(input); + return getY(); +} + +int blockHighPassTest() +{ + printf("Test BlockHighPass\t\t: "); + BlockHighPass highPass(NULL, "TEST_HP"); + // test initial state + ASSERT(equal(10.0f, highPass.getFCut())); + ASSERT(equal(0.0f, highPass.getU())); + ASSERT(equal(0.0f, highPass.getY())); + ASSERT(equal(0.0f, highPass.getDt())); + // set dt + highPass.setDt(0.1f); + ASSERT(equal(0.1f, highPass.getDt())); + // set state + highPass.setU(1.0f); + ASSERT(equal(1.0f, highPass.getU())); + highPass.setY(1.0f); + ASSERT(equal(1.0f, highPass.getY())); + // test update + ASSERT(equal(0.2746051f, highPass.update(2.0f))); + + // test end condition + for (int i = 0; i < 100; i++) { + highPass.update(2.0f); + } + + ASSERT(equal(0.0f, highPass.getY())); + ASSERT(equal(0.0f, highPass.update(2.0f))); + printf("PASS\n"); + return 0; +} + +float BlockIntegral::update(float input) +{ + // trapezoidal integration + setY(_limit.update(getY() + input * getDt())); + return getY(); +} + +int blockIntegralTest() +{ + printf("Test BlockIntegral\t\t: "); + BlockIntegral integral(NULL, "TEST_I"); + // test initial state + ASSERT(equal(1.0f, integral.getMax())); + ASSERT(equal(0.0f, integral.getDt())); + // set dt + integral.setDt(0.1f); + ASSERT(equal(0.1f, integral.getDt())); + // set Y + integral.setY(0.9f); + ASSERT(equal(0.9f, integral.getY())); + + // test exceed max + for (int i = 0; i < 100; i++) { + integral.update(1.0f); + } + + ASSERT(equal(1.0f, integral.update(1.0f))); + // test exceed min + integral.setY(-0.9f); + ASSERT(equal(-0.9f, integral.getY())); + + for (int i = 0; i < 100; i++) { + integral.update(-1.0f); + } + + ASSERT(equal(-1.0f, integral.update(-1.0f))); + // test update + integral.setY(0.1f); + ASSERT(equal(0.2f, integral.update(1.0))); + ASSERT(equal(0.2f, integral.getY())); + printf("PASS\n"); + return 0; +} + +float BlockIntegralTrap::update(float input) +{ + // trapezoidal integration + setY(_limit.update(getY() + + (getU() + input) / 2.0f * getDt())); + setU(input); + return getY(); +} + +int blockIntegralTrapTest() +{ + printf("Test BlockIntegralTrap\t\t: "); + BlockIntegralTrap integral(NULL, "TEST_I"); + // test initial state + ASSERT(equal(1.0f, integral.getMax())); + ASSERT(equal(0.0f, integral.getDt())); + // set dt + integral.setDt(0.1f); + ASSERT(equal(0.1f, integral.getDt())); + // set U + integral.setU(1.0f); + ASSERT(equal(1.0f, integral.getU())); + // set Y + integral.setY(0.9f); + ASSERT(equal(0.9f, integral.getY())); + + // test exceed max + for (int i = 0; i < 100; i++) { + integral.update(1.0f); + } + + ASSERT(equal(1.0f, integral.update(1.0f))); + // test exceed min + integral.setU(-1.0f); + integral.setY(-0.9f); + ASSERT(equal(-0.9f, integral.getY())); + + for (int i = 0; i < 100; i++) { + integral.update(-1.0f); + } + + ASSERT(equal(-1.0f, integral.update(-1.0f))); + // test update + integral.setU(2.0f); + integral.setY(0.1f); + ASSERT(equal(0.25f, integral.update(1.0))); + ASSERT(equal(0.25f, integral.getY())); + ASSERT(equal(1.0f, integral.getU())); + printf("PASS\n"); + return 0; +} + +float BlockDerivative::update(float input) +{ + float output = _lowPass.update((input - getU()) / getDt()); + setU(input); + return output; +} + +int blockDerivativeTest() +{ + printf("Test BlockDerivative\t\t: "); + BlockDerivative derivative(NULL, "TEST_D"); + // test initial state + ASSERT(equal(0.0f, derivative.getU())); + ASSERT(equal(10.0f, derivative.getLP())); + // set dt + derivative.setDt(0.1f); + ASSERT(equal(0.1f, derivative.getDt())); + // set U + derivative.setU(1.0f); + ASSERT(equal(1.0f, derivative.getU())); + // test update + ASSERT(equal(8.6269744f, derivative.update(2.0f))); + ASSERT(equal(2.0f, derivative.getU())); + printf("PASS\n"); + return 0; +} + +int blockPTest() +{ + printf("Test BlockP\t\t\t: "); + BlockP blockP(NULL, "TEST_P"); + // test initial state + ASSERT(equal(0.2f, blockP.getKP())); + ASSERT(equal(0.0f, blockP.getDt())); + // set dt + blockP.setDt(0.1f); + ASSERT(equal(0.1f, blockP.getDt())); + // test update + ASSERT(equal(0.4f, blockP.update(2.0f))); + printf("PASS\n"); + return 0; +} + +int blockPITest() +{ + printf("Test BlockPI\t\t\t: "); + BlockPI blockPI(NULL, "TEST"); + // test initial state + ASSERT(equal(0.2f, blockPI.getKP())); + ASSERT(equal(0.1f, blockPI.getKI())); + ASSERT(equal(0.0f, blockPI.getDt())); + ASSERT(equal(1.0f, blockPI.getIntegral().getMax())); + // set dt + blockPI.setDt(0.1f); + ASSERT(equal(0.1f, blockPI.getDt())); + // set integral state + blockPI.getIntegral().setY(0.1f); + ASSERT(equal(0.1f, blockPI.getIntegral().getY())); + // test update + // 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43 + ASSERT(equal(0.43f, blockPI.update(2.0f))); + printf("PASS\n"); + return 0; +} + +int blockPDTest() +{ + printf("Test BlockPD\t\t\t: "); + BlockPD blockPD(NULL, "TEST"); + // test initial state + ASSERT(equal(0.2f, blockPD.getKP())); + ASSERT(equal(0.01f, blockPD.getKD())); + ASSERT(equal(0.0f, blockPD.getDt())); + ASSERT(equal(10.0f, blockPD.getDerivative().getLP())); + // set dt + blockPD.setDt(0.1f); + ASSERT(equal(0.1f, blockPD.getDt())); + // set derivative state + blockPD.getDerivative().setU(1.0f); + ASSERT(equal(1.0f, blockPD.getDerivative().getU())); + // test update + // 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744 + ASSERT(equal(0.486269744f, blockPD.update(2.0f))); + printf("PASS\n"); + return 0; +} + +int blockPIDTest() +{ + printf("Test BlockPID\t\t\t: "); + BlockPID blockPID(NULL, "TEST"); + // test initial state + ASSERT(equal(0.2f, blockPID.getKP())); + ASSERT(equal(0.1f, blockPID.getKI())); + ASSERT(equal(0.01f, blockPID.getKD())); + ASSERT(equal(0.0f, blockPID.getDt())); + ASSERT(equal(10.0f, blockPID.getDerivative().getLP())); + ASSERT(equal(1.0f, blockPID.getIntegral().getMax())); + // set dt + blockPID.setDt(0.1f); + ASSERT(equal(0.1f, blockPID.getDt())); + // set derivative state + blockPID.getDerivative().setU(1.0f); + ASSERT(equal(1.0f, blockPID.getDerivative().getU())); + // set integral state + blockPID.getIntegral().setY(0.1f); + ASSERT(equal(0.1f, blockPID.getIntegral().getY())); + // test update + // 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697 + ASSERT(equal(0.5162697f, blockPID.update(2.0f))); + printf("PASS\n"); + return 0; +} + +int blockOutputTest() +{ + printf("Test BlockOutput\t\t: "); + BlockOutput blockOutput(NULL, "TEST"); + // test initial state + ASSERT(equal(0.0f, blockOutput.getDt())); + ASSERT(equal(0.5f, blockOutput.get())); + ASSERT(equal(-1.0f, blockOutput.getMin())); + ASSERT(equal(1.0f, blockOutput.getMax())); + // test update below min + blockOutput.update(-2.0f); + ASSERT(equal(-1.0f, blockOutput.get())); + // test update above max + blockOutput.update(2.0f); + ASSERT(equal(1.0f, blockOutput.get())); + // test trim + blockOutput.update(0.0f); + ASSERT(equal(0.5f, blockOutput.get())); + printf("PASS\n"); + return 0; +} + +int blockRandUniformTest() +{ + srand(1234); + printf("Test BlockRandUniform\t\t: "); + BlockRandUniform blockRandUniform(NULL, "TEST"); + // test initial state + ASSERT(equal(0.0f, blockRandUniform.getDt())); + ASSERT(equal(-1.0f, blockRandUniform.getMin())); + ASSERT(equal(1.0f, blockRandUniform.getMax())); + // test update + int n = 10000; + float mean = blockRandUniform.update(); + + // recursive mean algorithm from Knuth + for (int i = 2; i < n + 1; i++) { + float val = blockRandUniform.update(); + mean += (val - mean) / i; + ASSERT(val <= blockRandUniform.getMax()); + ASSERT(val >= blockRandUniform.getMin()); + } + + ASSERT(equal(mean, (blockRandUniform.getMin() + + blockRandUniform.getMax()) / 2, 1e-1)); + printf("PASS\n"); + return 0; +} + +int blockRandGaussTest() +{ + srand(1234); + printf("Test BlockRandGauss\t\t: "); + BlockRandGauss blockRandGauss(NULL, "TEST"); + // test initial state + ASSERT(equal(0.0f, blockRandGauss.getDt())); + ASSERT(equal(1.0f, blockRandGauss.getMean())); + ASSERT(equal(2.0f, blockRandGauss.getStdDev())); + // test update + int n = 10000; + float mean = blockRandGauss.update(); + float sum = 0; + + // recursive mean, stdev algorithm from Knuth + for (int i = 2; i < n + 1; i++) { + float val = blockRandGauss.update(); + float newMean = mean + (val - mean) / i; + sum += (val - mean) * (val - newMean); + mean = newMean; + } + + float stdDev = sqrt(sum / (n - 1)); + ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1)); + ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); + printf("PASS\n"); + return 0; +} + +} // namespace control -- cgit v1.2.3