PES2_R2D2.0 / Mbed 2 deprecated MicroMouse_MASTER_FOUR

Pathfinding nach rechts funktioniert noch nicht...der rest schon

Dependencies:   mbed

Fork of MicroMouse_MASTER_THREE by PES2_R2D2.0

LowpassFilter.cpp@1:d9e840c48b1e, 2018-03-31 (annotated)

Committer:
ruesipat
Date:
Sat Mar 31 16:45:57 2018 +0000
Revision:
1:d9e840c48b1e
Parent:
0:a9fe4ef404bf
Child:
2:592f01278db4
j

Who changed what in which revision?

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