Biorobotics_group_2 / Mbed 2 deprecated Encoder

Reads the encoder signals from 1 motor and displays the counts and degrees of the output shaft in Serial.

Dependencies:   HIDScope_motor_ff QEI mbed FastPWM MODSERIAL

Fork of HID_scope_test by Biorobotics_group_2

BiQuad.h@9:278d25dc0ef3, 2016-10-28 (annotated)

Committer:
sjoerdbarts
Date:
Fri Oct 28 09:58:22 2016 +0000
Revision:
9:278d25dc0ef3
Parent:
8:fe907b9a0bab 
Working code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sjoerdbarts 8:fe907b9a0bab 1 #ifndef BIQUAD_BIQUAD_H
sjoerdbarts 8:fe907b9a0bab 2 #define BIQUAD_BIQUAD_H
sjoerdbarts 8:fe907b9a0bab 3
sjoerdbarts 8:fe907b9a0bab 4 #include <vector>
sjoerdbarts 8:fe907b9a0bab 5 #include <complex>
sjoerdbarts 8:fe907b9a0bab 6
sjoerdbarts 8:fe907b9a0bab 7 class BiQuadChain;
sjoerdbarts 8:fe907b9a0bab 8
sjoerdbarts 8:fe907b9a0bab 9 /** BiQuad class implements a single filter
sjoerdbarts 8:fe907b9a0bab 10 *
sjoerdbarts 8:fe907b9a0bab 11 * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl>
sjoerdbarts 8:fe907b9a0bab 12 *
sjoerdbarts 8:fe907b9a0bab 13 * Filters that - in the z domain - are the ratio of two quadratic functions. The general form is:
sjoerdbarts 8:fe907b9a0bab 14 *
sjoerdbarts 8:fe907b9a0bab 15 * b0 + b1 z^-1 + b2 z^-2
sjoerdbarts 8:fe907b9a0bab 16 * H(z) = ----------------------
sjoerdbarts 8:fe907b9a0bab 17 * a0 + a1 z^-1 + a2 z^-2
sjoerdbarts 8:fe907b9a0bab 18 *
sjoerdbarts 8:fe907b9a0bab 19 * Which is often normalized by dividing all coefficients by a0.
sjoerdbarts 8:fe907b9a0bab 20 *
sjoerdbarts 8:fe907b9a0bab 21 * Example:
sjoerdbarts 8:fe907b9a0bab 22 * @code
sjoerdbarts 8:fe907b9a0bab 23 * #include "mbed.h"
sjoerdbarts 8:fe907b9a0bab 24 * #include <complex>
sjoerdbarts 8:fe907b9a0bab 25 *
sjoerdbarts 8:fe907b9a0bab 26 * // Example: 4th order Butterworth LP (w_c = 0.1*f_nyquist)
sjoerdbarts 8:fe907b9a0bab 27 * BiQuad bq1( 4.16599e-04, 8.33198e-04, 4.16599e-04, -1.47967e+00, 5.55822e-01 );
sjoerdbarts 8:fe907b9a0bab 28 * BiQuad bq2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.70096e+00, 7.88500e-01 );
sjoerdbarts 8:fe907b9a0bab 29 *
sjoerdbarts 8:fe907b9a0bab 30 * BiQuadChain bqc;
sjoerdbarts 8:fe907b9a0bab 31 *
sjoerdbarts 8:fe907b9a0bab 32 * int main() {
sjoerdbarts 8:fe907b9a0bab 33 *
sjoerdbarts 8:fe907b9a0bab 34 * // Add the biquads to the chain
sjoerdbarts 8:fe907b9a0bab 35 * bqc.add( &bq1 ).add( &bq2 );
sjoerdbarts 8:fe907b9a0bab 36 *
sjoerdbarts 8:fe907b9a0bab 37 * // Find the poles of the filter
sjoerdbarts 8:fe907b9a0bab 38 * std::cout << "Filter poles" << std::endl;
sjoerdbarts 8:fe907b9a0bab 39 * std::vector< std::complex<double> > poles = bqc.poles();
sjoerdbarts 8:fe907b9a0bab 40 * for( size_t i = 0; i < poles.size(); i++ )
sjoerdbarts 8:fe907b9a0bab 41 * std::cout << "\t" << poles[i] << std::endl;
sjoerdbarts 8:fe907b9a0bab 42 *
sjoerdbarts 8:fe907b9a0bab 43 * // Find the zeros of the filter
sjoerdbarts 8:fe907b9a0bab 44 * std::cout << "Filter zeros" << std::endl;
sjoerdbarts 8:fe907b9a0bab 45 * std::vector< std::complex<double> > zeros = bqc.zeros();
sjoerdbarts 8:fe907b9a0bab 46 * for( size_t i = 0; i < poles.size(); i++ )
sjoerdbarts 8:fe907b9a0bab 47 * std::cout << "\t" << zeros[i] << std::endl;
sjoerdbarts 8:fe907b9a0bab 48 *
sjoerdbarts 8:fe907b9a0bab 49 * // Is the filter stable?
sjoerdbarts 8:fe907b9a0bab 50 * std::cout << "This filter is " << (bqc.stable() ? "stable" : "instable") << std::endl;
sjoerdbarts 8:fe907b9a0bab 51 *
sjoerdbarts 8:fe907b9a0bab 52 * // Output the step-response of 20 samples
sjoerdbarts 8:fe907b9a0bab 53 * std::cout << "Step response 20 samples" << std::endl;
sjoerdbarts 8:fe907b9a0bab 54 * for( int i = 0; i < 20; i++ )
sjoerdbarts 8:fe907b9a0bab 55 * std::cout << "\t" << bqc.step( 1.0 ) << std::endl;
sjoerdbarts 8:fe907b9a0bab 56 * }
sjoerdbarts 8:fe907b9a0bab 57 * @endcode
sjoerdbarts 8:fe907b9a0bab 58 *
sjoerdbarts 8:fe907b9a0bab 59 * https://github.com/tomlankhorst/biquad
sjoerdbarts 8:fe907b9a0bab 60 *
sjoerdbarts 8:fe907b9a0bab 61 */
sjoerdbarts 8:fe907b9a0bab 62 class BiQuad {
sjoerdbarts 8:fe907b9a0bab 63
sjoerdbarts 8:fe907b9a0bab 64 private:
sjoerdbarts 8:fe907b9a0bab 65
sjoerdbarts 8:fe907b9a0bab 66 double B[3];
sjoerdbarts 8:fe907b9a0bab 67 double A[2];
sjoerdbarts 8:fe907b9a0bab 68 double wz[2];
sjoerdbarts 8:fe907b9a0bab 69
sjoerdbarts 8:fe907b9a0bab 70 bool resetStateOnGainChange;
sjoerdbarts 8:fe907b9a0bab 71
sjoerdbarts 8:fe907b9a0bab 72 /**
sjoerdbarts 8:fe907b9a0bab 73 * Sets the gain parameters
sjoerdbarts 8:fe907b9a0bab 74 */
sjoerdbarts 8:fe907b9a0bab 75 void set( double b0, double b1, double b2, double a1, double a2 );
sjoerdbarts 8:fe907b9a0bab 76
sjoerdbarts 8:fe907b9a0bab 77 public:
sjoerdbarts 8:fe907b9a0bab 78
sjoerdbarts 8:fe907b9a0bab 79 /**
sjoerdbarts 8:fe907b9a0bab 80 * Initialize a unity TF biquad
sjoerdbarts 8:fe907b9a0bab 81 * @return BiQuad instance
sjoerdbarts 8:fe907b9a0bab 82 */
sjoerdbarts 8:fe907b9a0bab 83 BiQuad( );
sjoerdbarts 8:fe907b9a0bab 84
sjoerdbarts 8:fe907b9a0bab 85 /**
sjoerdbarts 8:fe907b9a0bab 86 * Initialize a normalized biquad filter
sjoerdbarts 8:fe907b9a0bab 87 * @param b0
sjoerdbarts 8:fe907b9a0bab 88 * @param b1
sjoerdbarts 8:fe907b9a0bab 89 * @param b2
sjoerdbarts 8:fe907b9a0bab 90 * @param a1
sjoerdbarts 8:fe907b9a0bab 91 * @param a2
sjoerdbarts 8:fe907b9a0bab 92 * @return BiQuad instance
sjoerdbarts 8:fe907b9a0bab 93 */
sjoerdbarts 8:fe907b9a0bab 94 BiQuad( double b0, double b1, double b2, double a1, double a2 );
sjoerdbarts 8:fe907b9a0bab 95
sjoerdbarts 8:fe907b9a0bab 96 /**
sjoerdbarts 8:fe907b9a0bab 97 * Initialize a biquad filter with all six coefficients
sjoerdbarts 8:fe907b9a0bab 98 * @param b0
sjoerdbarts 8:fe907b9a0bab 99 * @param b1
sjoerdbarts 8:fe907b9a0bab 100 * @param b2
sjoerdbarts 8:fe907b9a0bab 101 * @param a0
sjoerdbarts 8:fe907b9a0bab 102 * @param a1
sjoerdbarts 8:fe907b9a0bab 103 * @param a2
sjoerdbarts 8:fe907b9a0bab 104 * @return BiQuad instance
sjoerdbarts 8:fe907b9a0bab 105 */
sjoerdbarts 8:fe907b9a0bab 106 BiQuad( double b0, double b1, double b2, double a0, double a1, double a2 );
sjoerdbarts 8:fe907b9a0bab 107
sjoerdbarts 8:fe907b9a0bab 108 /**
sjoerdbarts 8:fe907b9a0bab 109 * Initialize a PIDF biquad.
sjoerdbarts 8:fe907b9a0bab 110 * Based on Tustin-approx (trapezoidal) of the continous time version.
sjoerdbarts 8:fe907b9a0bab 111 * Behaviour equivalent to the PID controller created with the following MATLAB expression:
sjoerdbarts 8:fe907b9a0bab 112 *
sjoerdbarts 8:fe907b9a0bab 113 * C = pid( Kp, Ki, Kd, 1/N, Ts, 'IFormula', 'Trapezoidal', 'DFormula', 'Trapezoidal' );
sjoerdbarts 8:fe907b9a0bab 114 *
sjoerdbarts 8:fe907b9a0bab 115 * @param Kp Proportional gain
sjoerdbarts 8:fe907b9a0bab 116 * @param Ki Integral gain
sjoerdbarts 8:fe907b9a0bab 117 * @param Kd Derivative gain
sjoerdbarts 8:fe907b9a0bab 118 * @param N Filter coefficient ( N = 1/Tf )
sjoerdbarts 8:fe907b9a0bab 119 * @param Ts Timestep
sjoerdbarts 8:fe907b9a0bab 120 */
sjoerdbarts 8:fe907b9a0bab 121 void PIDF( double Kp, double Ki, double Kd, double N, double Ts );
sjoerdbarts 8:fe907b9a0bab 122
sjoerdbarts 8:fe907b9a0bab 123 /**
sjoerdbarts 8:fe907b9a0bab 124 * Execute one digital timestep and return the result...
sjoerdbarts 8:fe907b9a0bab 125 * @param x input of the filer
sjoerdbarts 8:fe907b9a0bab 126 * @return output of the filter
sjoerdbarts 8:fe907b9a0bab 127 */
sjoerdbarts 8:fe907b9a0bab 128 double step( double x );
sjoerdbarts 8:fe907b9a0bab 129
sjoerdbarts 8:fe907b9a0bab 130 /**
sjoerdbarts 8:fe907b9a0bab 131 * Return poles of the BiQuad filter
sjoerdbarts 8:fe907b9a0bab 132 * @return vector of std::complex poles
sjoerdbarts 8:fe907b9a0bab 133 */
sjoerdbarts 8:fe907b9a0bab 134 std::vector< std::complex<double> > poles( );
sjoerdbarts 8:fe907b9a0bab 135
sjoerdbarts 8:fe907b9a0bab 136 /**
sjoerdbarts 8:fe907b9a0bab 137 * Return zeros of the BiQuad filter
sjoerdbarts 8:fe907b9a0bab 138 * @return vector of std::complex zeros
sjoerdbarts 8:fe907b9a0bab 139 */
sjoerdbarts 8:fe907b9a0bab 140 std::vector< std::complex<double> > zeros( );
sjoerdbarts 8:fe907b9a0bab 141
sjoerdbarts 8:fe907b9a0bab 142 /**
sjoerdbarts 8:fe907b9a0bab 143 * Is this biquad stable?
sjoerdbarts 8:fe907b9a0bab 144 * Checks if all poles lie within the unit-circle
sjoerdbarts 8:fe907b9a0bab 145 * @return boolean whether the filter is stable or not
sjoerdbarts 8:fe907b9a0bab 146 */
sjoerdbarts 8:fe907b9a0bab 147 bool stable ();
sjoerdbarts 8:fe907b9a0bab 148
sjoerdbarts 8:fe907b9a0bab 149 /**
sjoerdbarts 8:fe907b9a0bab 150 * Determines if the state variables are reset to zero on gain change.
sjoerdbarts 8:fe907b9a0bab 151 * Can be used for changing gain parameters on the fly.
sjoerdbarts 8:fe907b9a0bab 152 * @param v Value of the reset boolean
sjoerdbarts 8:fe907b9a0bab 153 */
sjoerdbarts 8:fe907b9a0bab 154 void setResetStateOnGainChange( bool v );
sjoerdbarts 8:fe907b9a0bab 155
sjoerdbarts 8:fe907b9a0bab 156 };
sjoerdbarts 8:fe907b9a0bab 157
sjoerdbarts 8:fe907b9a0bab 158 /**
sjoerdbarts 8:fe907b9a0bab 159 * The BiQuadChain class implements a chain of BiQuad filters
sjoerdbarts 8:fe907b9a0bab 160 */
sjoerdbarts 8:fe907b9a0bab 161 class BiQuadChain {
sjoerdbarts 8:fe907b9a0bab 162
sjoerdbarts 8:fe907b9a0bab 163 private:
sjoerdbarts 8:fe907b9a0bab 164 std::vector< BiQuad* > biquads;
sjoerdbarts 8:fe907b9a0bab 165 std::vector< std::complex<double> > poles_zeros( bool zeros = false );
sjoerdbarts 8:fe907b9a0bab 166
sjoerdbarts 8:fe907b9a0bab 167 public:
sjoerdbarts 8:fe907b9a0bab 168
sjoerdbarts 8:fe907b9a0bab 169 /**
sjoerdbarts 8:fe907b9a0bab 170 * Add a BiQuad pointer to the list: bqc.add(&bq);
sjoerdbarts 8:fe907b9a0bab 171 * @param bq Pointer to BiQuad instance
sjoerdbarts 8:fe907b9a0bab 172 * @return Pointer to BiQuadChain
sjoerdbarts 8:fe907b9a0bab 173 */
sjoerdbarts 8:fe907b9a0bab 174 BiQuadChain &add( BiQuad *bq );
sjoerdbarts 8:fe907b9a0bab 175
sjoerdbarts 8:fe907b9a0bab 176 /**
sjoerdbarts 8:fe907b9a0bab 177 * Execute a digital time step cascaded through all bq's
sjoerdbarts 8:fe907b9a0bab 178 * @param x Input of the filter chain
sjoerdbarts 8:fe907b9a0bab 179 * @return Output of the chain
sjoerdbarts 8:fe907b9a0bab 180 */
sjoerdbarts 8:fe907b9a0bab 181 double step(double x);
sjoerdbarts 8:fe907b9a0bab 182
sjoerdbarts 8:fe907b9a0bab 183 /**
sjoerdbarts 8:fe907b9a0bab 184 * Return poles of the BiQuad filter
sjoerdbarts 8:fe907b9a0bab 185 * @return vector of std::complex poles
sjoerdbarts 8:fe907b9a0bab 186 */
sjoerdbarts 8:fe907b9a0bab 187 std::vector< std::complex<double> > poles( );
sjoerdbarts 8:fe907b9a0bab 188
sjoerdbarts 8:fe907b9a0bab 189 /**
sjoerdbarts 8:fe907b9a0bab 190 * Return zeros of the BiQuad filter
sjoerdbarts 8:fe907b9a0bab 191 * @return vector of std::complex zeros
sjoerdbarts 8:fe907b9a0bab 192 */
sjoerdbarts 8:fe907b9a0bab 193 std::vector< std::complex<double> > zeros( );
sjoerdbarts 8:fe907b9a0bab 194
sjoerdbarts 8:fe907b9a0bab 195 /**
sjoerdbarts 8:fe907b9a0bab 196 * Is this biquad-chain stable?
sjoerdbarts 8:fe907b9a0bab 197 * Checks if all poles lie within the unit-circle
sjoerdbarts 8:fe907b9a0bab 198 * @return boolean whether the chain is stable or not
sjoerdbarts 8:fe907b9a0bab 199 */
sjoerdbarts 8:fe907b9a0bab 200 bool stable ();
sjoerdbarts 8:fe907b9a0bab 201
sjoerdbarts 8:fe907b9a0bab 202 /**
sjoerdbarts 8:fe907b9a0bab 203 * Appends a BiQuad to the chain
sjoerdbarts 8:fe907b9a0bab 204 * Shorthand for .add(&bq)
sjoerdbarts 8:fe907b9a0bab 205 * @param bq BiQuad
sjoerdbarts 8:fe907b9a0bab 206 * @return Pointer to BiQuadChain
sjoerdbarts 8:fe907b9a0bab 207 */
sjoerdbarts 8:fe907b9a0bab 208 BiQuadChain &operator*( BiQuad& bq );
sjoerdbarts 8:fe907b9a0bab 209
sjoerdbarts 8:fe907b9a0bab 210 };
sjoerdbarts 8:fe907b9a0bab 211
sjoerdbarts 8:fe907b9a0bab 212 /**
sjoerdbarts 8:fe907b9a0bab 213 * Multiply two BiQuads
sjoerdbarts 8:fe907b9a0bab 214 * ... which in fact means appending them into a BiQuadChain
sjoerdbarts 8:fe907b9a0bab 215 * @return BiQuadChain of the two BiQuads
sjoerdbarts 8:fe907b9a0bab 216 */
sjoerdbarts 8:fe907b9a0bab 217 BiQuadChain operator*( BiQuad&, BiQuad& );
sjoerdbarts 8:fe907b9a0bab 218
sjoerdbarts 8:fe907b9a0bab 219 #endif //BIQUAD_BIQUAD_H