Helvijs Kiselis / Mbed 2 deprecated MicromousePES2

main.cpp: Sensoren einlesen und Motoren ansteuern

Dependencies:   mbed

LowpassFilter.cpp@8:862bf9225953, 2018-04-16 (annotated)

Committer:
luethale
Date:
Mon Apr 16 15:23:42 2018 +0000
Revision:
8:862bf9225953
Parent:
0:9a3e7847a4be 
Regelung f?r Spurhaltung

Who changed what in which revision?

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