kings / Mbed 2 deprecated biszueimchlotz

Drehen mit Halt und offset um zum Klotz zurück drehen. (Kann nur ein klotz aufheben)

Dependencies:   Servo mbed

Fork of DrehungMitStopp by kings

LowpassFilter.cpp@4:85b8b4aa97a3, 2017-05-18 (annotated)

Committer:
freunjor
Date:
Thu May 18 13:25:31 2017 +0000
Revision:
4:85b8b4aa97a3
Parent:
1:d40ff07e2fe0 
fdasdf

Who changed what in which revision?

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