Gruppe 3 / Mbed 2 deprecated PES3

hallo

Dependencies:   Servo mbed pixy

Fork of PES1 by Gruppe 3

EncoderCounter/LowpassFilter.cpp@18:3ee1b02ed3aa, 2017-05-09 (annotated)

Committer:
itslinear
Date:
Tue May 09 15:25:54 2017 +0000
Revision:
18:3ee1b02ed3aa
new

Who changed what in which revision?

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