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@0:a9fe4ef404bf, 2018-03-07 (annotated)

Committer:
ruesipat
Date:
Wed Mar 07 14:06:19 2018 +0000
Revision:
0:a9fe4ef404bf
Child:
1:d9e840c48b1e
hallo

Who changed what in which revision?

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