Mechatronics Robotics / Mbed 2 deprecated BrobotV1

These are the core files for the Robot at Team conception.

Dependencies:   mbed UniServ

LowpassFilter.cpp@17:ec52258b9472, 2017-06-07 (annotated)

Committer:
obrie829
Date:
Wed Jun 07 11:35:59 2017 +0000
Revision:
17:ec52258b9472
Parent:
2:644553c019c5 
v18

Who changed what in which revision?

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