Mechatronics Robotics / EncoderCounter

with speed control, to be merged with pixy stuff

LowpassFilter.cpp@0:f669d57505a0, 2017-05-29 (annotated)

Committer:
obrie829
Date:
Mon May 29 13:03:17 2017 +0000
Revision:
0:f669d57505a0
with speed control class

Who changed what in which revision?

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