Biorobotics_group_2 / Mbed 2 deprecated Berekenen_motorhoek

Diagonaal berekenen lukt, meerdere outputs nog niet

Dependencies:   mbed FastPWM HIDScope MODSERIAL QEI

BiQuad.h@6:ee243782bf51, 2016-10-20 (annotated)

Committer:
sjoerdbarts
Date:
Thu Oct 20 12:07:30 2016 +0000
Revision:
6:ee243782bf51
Initial working code

Who changed what in which revision?

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