Basis aansturing projectgroep 3
Dependencies: Biquad HIDScope MODSERIAL QEI mbed
BiQuad.cpp@9:e764cb50d343, 2016-11-07 (annotated)
- Committer:
- s1588141
- Date:
- Mon Nov 07 13:30:49 2016 +0000
- Revision:
- 9:e764cb50d343
- Parent:
- 5:4b2ff2a4664a
Final program;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
s1588141 | 5:4b2ff2a4664a | 1 | #include "BiQuad.h" |
s1588141 | 5:4b2ff2a4664a | 2 | |
s1588141 | 5:4b2ff2a4664a | 3 | BiQuad::BiQuad() { |
s1588141 | 5:4b2ff2a4664a | 4 | resetStateOnGainChange = true; |
s1588141 | 5:4b2ff2a4664a | 5 | set( 1.0, 0.0, 0.0, 0.0, 0.0 ); |
s1588141 | 5:4b2ff2a4664a | 6 | } |
s1588141 | 5:4b2ff2a4664a | 7 | |
s1588141 | 5:4b2ff2a4664a | 8 | BiQuad::BiQuad(double b0, double b1, double b2, double a1, double a2) { |
s1588141 | 5:4b2ff2a4664a | 9 | resetStateOnGainChange = true; |
s1588141 | 5:4b2ff2a4664a | 10 | set( b0, b1, b2, a1, a2 ); |
s1588141 | 5:4b2ff2a4664a | 11 | } |
s1588141 | 5:4b2ff2a4664a | 12 | |
s1588141 | 5:4b2ff2a4664a | 13 | BiQuad::BiQuad(double b0, double b1, double b2, double a0, double a1, double a2) { |
s1588141 | 5:4b2ff2a4664a | 14 | resetStateOnGainChange = true; |
s1588141 | 5:4b2ff2a4664a | 15 | set( b0/a0, b1/a0, b2/a0, a1/a0, a2/a0 ); |
s1588141 | 5:4b2ff2a4664a | 16 | } |
s1588141 | 5:4b2ff2a4664a | 17 | |
s1588141 | 5:4b2ff2a4664a | 18 | void BiQuad::PIDF( double Kp, double Ki, double Kd, double N, double Ts ) { |
s1588141 | 5:4b2ff2a4664a | 19 | |
s1588141 | 5:4b2ff2a4664a | 20 | double b0, b1, b2, bd, a1, a2; |
s1588141 | 5:4b2ff2a4664a | 21 | |
s1588141 | 5:4b2ff2a4664a | 22 | a1 = -4.0/(N*Ts+2.0); |
s1588141 | 5:4b2ff2a4664a | 23 | a2 = -(N*Ts-2.0)/(N*Ts+2.0); |
s1588141 | 5:4b2ff2a4664a | 24 | |
s1588141 | 5:4b2ff2a4664a | 25 | bd = ( N*Ts+2.0 ); |
s1588141 | 5:4b2ff2a4664a | 26 | |
s1588141 | 5:4b2ff2a4664a | 27 | b0 = ( 4.0*Kp + 4.0*Kd*N + 2.0*Ki*Ts + 2.0*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd); |
s1588141 | 5:4b2ff2a4664a | 28 | b1 = ( Ki*N*Ts*Ts - 4.0*Kp - 4.0*Kd*N )/bd; |
s1588141 | 5:4b2ff2a4664a | 29 | b2 = ( 4.0*Kp + 4.0*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd); |
s1588141 | 5:4b2ff2a4664a | 30 | |
s1588141 | 5:4b2ff2a4664a | 31 | set( b0, b1, b2, a1, a2 ); |
s1588141 | 5:4b2ff2a4664a | 32 | |
s1588141 | 5:4b2ff2a4664a | 33 | }; |
s1588141 | 5:4b2ff2a4664a | 34 | |
s1588141 | 5:4b2ff2a4664a | 35 | void BiQuad::set(double b0, double b1, double b2, double a1, double a2) { |
s1588141 | 5:4b2ff2a4664a | 36 | |
s1588141 | 5:4b2ff2a4664a | 37 | B[0] = b0; B[1] = b1; B[2] = b2; |
s1588141 | 5:4b2ff2a4664a | 38 | A[0] = a1; A[1] = a2; |
s1588141 | 5:4b2ff2a4664a | 39 | |
s1588141 | 5:4b2ff2a4664a | 40 | if( resetStateOnGainChange ) |
s1588141 | 5:4b2ff2a4664a | 41 | wz[0] = 0; wz[1] = 0; |
s1588141 | 5:4b2ff2a4664a | 42 | |
s1588141 | 5:4b2ff2a4664a | 43 | } |
s1588141 | 5:4b2ff2a4664a | 44 | |
s1588141 | 5:4b2ff2a4664a | 45 | double BiQuad::step(double x) { |
s1588141 | 5:4b2ff2a4664a | 46 | |
s1588141 | 5:4b2ff2a4664a | 47 | double y,w; |
s1588141 | 5:4b2ff2a4664a | 48 | |
s1588141 | 5:4b2ff2a4664a | 49 | /* Direct form II */ |
s1588141 | 5:4b2ff2a4664a | 50 | w = x - A[0]*wz[0] - A[1]*wz[1]; |
s1588141 | 5:4b2ff2a4664a | 51 | y = B[0]*w + B[1]*wz[0] + B[2]*wz[1]; |
s1588141 | 5:4b2ff2a4664a | 52 | |
s1588141 | 5:4b2ff2a4664a | 53 | /* Shift */ |
s1588141 | 5:4b2ff2a4664a | 54 | wz[1] = wz[0]; |
s1588141 | 5:4b2ff2a4664a | 55 | wz[0] = w; |
s1588141 | 5:4b2ff2a4664a | 56 | |
s1588141 | 5:4b2ff2a4664a | 57 | return y; |
s1588141 | 5:4b2ff2a4664a | 58 | |
s1588141 | 5:4b2ff2a4664a | 59 | } |
s1588141 | 5:4b2ff2a4664a | 60 | |
s1588141 | 5:4b2ff2a4664a | 61 | std::vector< std::complex<double> > BiQuad::poles() { |
s1588141 | 5:4b2ff2a4664a | 62 | |
s1588141 | 5:4b2ff2a4664a | 63 | std::vector< std::complex<double> > poles; |
s1588141 | 5:4b2ff2a4664a | 64 | |
s1588141 | 5:4b2ff2a4664a | 65 | std::complex<double> b2(A[0]*A[0],0); |
s1588141 | 5:4b2ff2a4664a | 66 | std::complex<double> ds = std::sqrt( b2-4*A[1] ); |
s1588141 | 5:4b2ff2a4664a | 67 | |
s1588141 | 5:4b2ff2a4664a | 68 | poles.push_back( 0.5*(-A[0]+ds) ); |
s1588141 | 5:4b2ff2a4664a | 69 | poles.push_back( 0.5*(-A[0]-ds) ); |
s1588141 | 5:4b2ff2a4664a | 70 | |
s1588141 | 5:4b2ff2a4664a | 71 | return poles; |
s1588141 | 5:4b2ff2a4664a | 72 | |
s1588141 | 5:4b2ff2a4664a | 73 | } |
s1588141 | 5:4b2ff2a4664a | 74 | |
s1588141 | 5:4b2ff2a4664a | 75 | std::vector< std::complex<double> > BiQuad::zeros() { |
s1588141 | 5:4b2ff2a4664a | 76 | |
s1588141 | 5:4b2ff2a4664a | 77 | std::vector< std::complex<double> > zeros; |
s1588141 | 5:4b2ff2a4664a | 78 | |
s1588141 | 5:4b2ff2a4664a | 79 | std::complex<double> b2(B[1]*B[1],0); |
s1588141 | 5:4b2ff2a4664a | 80 | std::complex<double> ds = std::sqrt( b2-4*B[0]*B[2] ); |
s1588141 | 5:4b2ff2a4664a | 81 | |
s1588141 | 5:4b2ff2a4664a | 82 | zeros.push_back( 0.5*(-B[1]+ds)/B[0] ); |
s1588141 | 5:4b2ff2a4664a | 83 | zeros.push_back( 0.5*(-B[1]-ds)/B[0] ); |
s1588141 | 5:4b2ff2a4664a | 84 | |
s1588141 | 5:4b2ff2a4664a | 85 | return zeros; |
s1588141 | 5:4b2ff2a4664a | 86 | |
s1588141 | 5:4b2ff2a4664a | 87 | } |
s1588141 | 5:4b2ff2a4664a | 88 | |
s1588141 | 5:4b2ff2a4664a | 89 | bool BiQuad::stable() { |
s1588141 | 5:4b2ff2a4664a | 90 | bool stable = true; |
s1588141 | 5:4b2ff2a4664a | 91 | std::vector< std::complex<double> > ps = poles(); |
s1588141 | 5:4b2ff2a4664a | 92 | for( size_t i = 0; i < ps.size(); i++ ) |
s1588141 | 5:4b2ff2a4664a | 93 | stable = stable & ( std::abs( ps[i] ) < 1 ); |
s1588141 | 5:4b2ff2a4664a | 94 | return stable; |
s1588141 | 5:4b2ff2a4664a | 95 | } |
s1588141 | 5:4b2ff2a4664a | 96 | |
s1588141 | 5:4b2ff2a4664a | 97 | void BiQuad::setResetStateOnGainChange( bool v ){ |
s1588141 | 5:4b2ff2a4664a | 98 | resetStateOnGainChange = v; |
s1588141 | 5:4b2ff2a4664a | 99 | } |
s1588141 | 5:4b2ff2a4664a | 100 | |
s1588141 | 5:4b2ff2a4664a | 101 | BiQuadChain &BiQuadChain::add(BiQuad *bq) { |
s1588141 | 5:4b2ff2a4664a | 102 | biquads.push_back( bq ); |
s1588141 | 5:4b2ff2a4664a | 103 | return *this; |
s1588141 | 5:4b2ff2a4664a | 104 | } |
s1588141 | 5:4b2ff2a4664a | 105 | |
s1588141 | 5:4b2ff2a4664a | 106 | BiQuadChain operator*( BiQuad &bq1, BiQuad &bq2 ) { |
s1588141 | 5:4b2ff2a4664a | 107 | BiQuadChain bqc; |
s1588141 | 5:4b2ff2a4664a | 108 | bqc.add( &bq1 ).add( &bq2 ); |
s1588141 | 5:4b2ff2a4664a | 109 | return bqc; |
s1588141 | 5:4b2ff2a4664a | 110 | } |
s1588141 | 5:4b2ff2a4664a | 111 | |
s1588141 | 5:4b2ff2a4664a | 112 | double BiQuadChain::step(double x) { |
s1588141 | 5:4b2ff2a4664a | 113 | |
s1588141 | 5:4b2ff2a4664a | 114 | int i; |
s1588141 | 5:4b2ff2a4664a | 115 | size_t bqs; |
s1588141 | 5:4b2ff2a4664a | 116 | |
s1588141 | 5:4b2ff2a4664a | 117 | bqs = biquads.size(); |
s1588141 | 5:4b2ff2a4664a | 118 | |
s1588141 | 5:4b2ff2a4664a | 119 | for( i = 0; i < bqs; i++ ) |
s1588141 | 5:4b2ff2a4664a | 120 | x = biquads[i]->step( x ); |
s1588141 | 5:4b2ff2a4664a | 121 | |
s1588141 | 5:4b2ff2a4664a | 122 | return x; |
s1588141 | 5:4b2ff2a4664a | 123 | } |
s1588141 | 5:4b2ff2a4664a | 124 | |
s1588141 | 5:4b2ff2a4664a | 125 | std::vector< std::complex<double> > BiQuadChain::poles_zeros( bool zeros ) { |
s1588141 | 5:4b2ff2a4664a | 126 | |
s1588141 | 5:4b2ff2a4664a | 127 | std::vector< std::complex<double> > chain, bq; |
s1588141 | 5:4b2ff2a4664a | 128 | int i; |
s1588141 | 5:4b2ff2a4664a | 129 | size_t bqs; |
s1588141 | 5:4b2ff2a4664a | 130 | |
s1588141 | 5:4b2ff2a4664a | 131 | bqs = biquads.size(); |
s1588141 | 5:4b2ff2a4664a | 132 | |
s1588141 | 5:4b2ff2a4664a | 133 | for( i = 0; i < bqs; i++ ){ |
s1588141 | 5:4b2ff2a4664a | 134 | bq = ( zeros ) ? biquads[ i ]->zeros() : biquads[ i ]->poles(); |
s1588141 | 5:4b2ff2a4664a | 135 | chain.insert( chain.end(), bq.begin(), bq.end() ); |
s1588141 | 5:4b2ff2a4664a | 136 | } |
s1588141 | 5:4b2ff2a4664a | 137 | |
s1588141 | 5:4b2ff2a4664a | 138 | return chain; |
s1588141 | 5:4b2ff2a4664a | 139 | |
s1588141 | 5:4b2ff2a4664a | 140 | } |
s1588141 | 5:4b2ff2a4664a | 141 | |
s1588141 | 5:4b2ff2a4664a | 142 | std::vector< std::complex<double> > BiQuadChain::poles() { |
s1588141 | 5:4b2ff2a4664a | 143 | return poles_zeros( false ); |
s1588141 | 5:4b2ff2a4664a | 144 | } |
s1588141 | 5:4b2ff2a4664a | 145 | |
s1588141 | 5:4b2ff2a4664a | 146 | std::vector< std::complex<double> > BiQuadChain::zeros() { |
s1588141 | 5:4b2ff2a4664a | 147 | return poles_zeros( true ); |
s1588141 | 5:4b2ff2a4664a | 148 | } |
s1588141 | 5:4b2ff2a4664a | 149 | |
s1588141 | 5:4b2ff2a4664a | 150 | bool BiQuadChain::stable() { |
s1588141 | 5:4b2ff2a4664a | 151 | bool stable = true; |
s1588141 | 5:4b2ff2a4664a | 152 | for( size_t i = 0; i < biquads.size(); i++ ) |
s1588141 | 5:4b2ff2a4664a | 153 | stable = stable & biquads[i]->stable(); |
s1588141 | 5:4b2ff2a4664a | 154 | return stable; |
s1588141 | 5:4b2ff2a4664a | 155 | } |
s1588141 | 5:4b2ff2a4664a | 156 | |
s1588141 | 5:4b2ff2a4664a | 157 | BiQuadChain& BiQuadChain::operator*( BiQuad& bq ) { |
s1588141 | 5:4b2ff2a4664a | 158 | add( &bq ); |
s1588141 | 5:4b2ff2a4664a | 159 | return *this; |
s1588141 | 5:4b2ff2a4664a | 160 | } |