zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_PIXY_Official

k

Dependencies:   Servo ServoArm mbed

Sources/LowpassFilter.cpp@0:15a8480061e8, 2017-05-22 (annotated)

Committer:
beacon
Date:
Mon May 22 11:24:46 2017 +0000
Revision:
0:15a8480061e8
o

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 0:15a8480061e8 1 /*
beacon 0:15a8480061e8 2 * LowpassFilter.cpp
beacon 0:15a8480061e8 3 * Copyright (c) 2016, ZHAW
beacon 0:15a8480061e8 4 * All rights reserved.
beacon 0:15a8480061e8 5 */
beacon 0:15a8480061e8 6
beacon 0:15a8480061e8 7 #include "Robot.h"
beacon 0:15a8480061e8 8 #include "Declarations.h"
beacon 0:15a8480061e8 9
beacon 0:15a8480061e8 10 using namespace std;
beacon 0:15a8480061e8 11
beacon 0:15a8480061e8 12 /**
beacon 0:15a8480061e8 13 * Creates a LowpassFilter object with a default corner frequency of 1000 [rad/s].
beacon 0:15a8480061e8 14 */
beacon 0:15a8480061e8 15 LowpassFilter::LowpassFilter() {
beacon 0:15a8480061e8 16
beacon 0:15a8480061e8 17 period = 1.0f;
beacon 0:15a8480061e8 18 frequency = 1000.0f;
beacon 0:15a8480061e8 19
beacon 0:15a8480061e8 20 a11 = (1.0f+frequency*period)*exp(-frequency*period);
beacon 0:15a8480061e8 21 a12 = period*exp(-frequency*period);
beacon 0:15a8480061e8 22 a21 = -frequency*frequency*period*exp(-frequency*period);
beacon 0:15a8480061e8 23 a22 = (1.0f-frequency*period)*exp(-frequency*period);
beacon 0:15a8480061e8 24 b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
beacon 0:15a8480061e8 25 b2 = period*exp(-frequency*period);
beacon 0:15a8480061e8 26
beacon 0:15a8480061e8 27 x1 = 0.0f;
beacon 0:15a8480061e8 28 x2 = 0.0f;
beacon 0:15a8480061e8 29 }
beacon 0:15a8480061e8 30
beacon 0:15a8480061e8 31 /**
beacon 0:15a8480061e8 32 * Deletes the LowpassFilter object.
beacon 0:15a8480061e8 33 */
beacon 0:15a8480061e8 34 LowpassFilter::~LowpassFilter() {}
beacon 0:15a8480061e8 35
beacon 0:15a8480061e8 36 /**
beacon 0:15a8480061e8 37 * Resets the filtered value to zero.
beacon 0:15a8480061e8 38 */
beacon 0:15a8480061e8 39 void LowpassFilter::reset() {
beacon 0:15a8480061e8 40
beacon 0:15a8480061e8 41 x1 = 0.0f;
beacon 0:15a8480061e8 42 x2 = 0.0f;
beacon 0:15a8480061e8 43 }
beacon 0:15a8480061e8 44
beacon 0:15a8480061e8 45 /**
beacon 0:15a8480061e8 46 * Resets the filtered value to a given value.
beacon 0:15a8480061e8 47 * @param value the value to reset the filter to.
beacon 0:15a8480061e8 48 */
beacon 0:15a8480061e8 49 void LowpassFilter::reset(float value) {
beacon 0:15a8480061e8 50
beacon 0:15a8480061e8 51 x1 = value/frequency/frequency;
beacon 0:15a8480061e8 52 x2 = (x1-a11*x1-b1*value)/a12;
beacon 0:15a8480061e8 53 }
beacon 0:15a8480061e8 54
beacon 0:15a8480061e8 55 /**
beacon 0:15a8480061e8 56 * Sets the sampling period of the filter.
beacon 0:15a8480061e8 57 * This is typically the sampling period of the realtime thread of a controller that uses this filter.
beacon 0:15a8480061e8 58 * @param the sampling period, given in [s].
beacon 0:15a8480061e8 59 */
beacon 0:15a8480061e8 60 void LowpassFilter::setPeriod(float period) {
beacon 0:15a8480061e8 61
beacon 0:15a8480061e8 62 this->period = period;
beacon 0:15a8480061e8 63
beacon 0:15a8480061e8 64 a11 = (1.0f+frequency*period)*exp(-frequency*period);
beacon 0:15a8480061e8 65 a12 = period*exp(-frequency*period);
beacon 0:15a8480061e8 66 a21 = -frequency*frequency*period*exp(-frequency*period);
beacon 0:15a8480061e8 67 a22 = (1.0f-frequency*period)*exp(-frequency*period);
beacon 0:15a8480061e8 68 b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
beacon 0:15a8480061e8 69 b2 = period*exp(-frequency*period);
beacon 0:15a8480061e8 70 }
beacon 0:15a8480061e8 71
beacon 0:15a8480061e8 72 /**
beacon 0:15a8480061e8 73 * Sets the corner frequency of this filter.
beacon 0:15a8480061e8 74 * @param frequency the corner frequency of the filter in [rad/s].
beacon 0:15a8480061e8 75 */
beacon 0:15a8480061e8 76 void LowpassFilter::setFrequency(float frequency) {
beacon 0:15a8480061e8 77
beacon 0:15a8480061e8 78 this->frequency = frequency;
beacon 0:15a8480061e8 79
beacon 0:15a8480061e8 80 a11 = (1.0f+frequency*period)*exp(-frequency*period);
beacon 0:15a8480061e8 81 a12 = period*exp(-frequency*period);
beacon 0:15a8480061e8 82 a21 = -frequency*frequency*period*exp(-frequency*period);
beacon 0:15a8480061e8 83 a22 = (1.0f-frequency*period)*exp(-frequency*period);
beacon 0:15a8480061e8 84 b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
beacon 0:15a8480061e8 85 b2 = period*exp(-frequency*period);
beacon 0:15a8480061e8 86 }
beacon 0:15a8480061e8 87
beacon 0:15a8480061e8 88 /**
beacon 0:15a8480061e8 89 * Gets the current corner frequency of this filter.
beacon 0:15a8480061e8 90 * @return the current corner frequency in [rad/s].
beacon 0:15a8480061e8 91 */
beacon 0:15a8480061e8 92 float LowpassFilter::getFrequency() {
beacon 0:15a8480061e8 93
beacon 0:15a8480061e8 94 return frequency;
beacon 0:15a8480061e8 95 }
beacon 0:15a8480061e8 96
beacon 0:15a8480061e8 97 /**
beacon 0:15a8480061e8 98 * Filters a value.
beacon 0:15a8480061e8 99 * @param value the original unfiltered value.
beacon 0:15a8480061e8 100 * @return the filtered value.
beacon 0:15a8480061e8 101 */
beacon 0:15a8480061e8 102 float LowpassFilter::filter(float value) {
beacon 0:15a8480061e8 103
beacon 0:15a8480061e8 104 float x1old = x1;
beacon 0:15a8480061e8 105 float x2old = x2;
beacon 0:15a8480061e8 106
beacon 0:15a8480061e8 107 x1 = a11*x1old+a12*x2old+b1*value;
beacon 0:15a8480061e8 108 x2 = a21*x1old+a22*x2old+b2*value;
beacon 0:15a8480061e8 109
beacon 0:15a8480061e8 110 return frequency*frequency*x1;
beacon 0:15a8480061e8 111 }