Basis aansturing projectgroep 3
Dependencies: Biquad HIDScope MODSERIAL QEI mbed
BiQuadChain.h
00001 #include "BiQuad.h" 00002 00003 BiQuad::BiQuad() { 00004 resetStateOnGainChange = true; 00005 set( 1.0, 0.0, 0.0, 0.0, 0.0 ); 00006 } 00007 00008 BiQuad::BiQuad(double b0, double b1, double b2, double a1, double a2) { 00009 resetStateOnGainChange = true; 00010 set( b0, b1, b2, a1, a2 ); 00011 } 00012 00013 BiQuad::BiQuad(double b0, double b1, double b2, double a0, double a1, double a2) { 00014 resetStateOnGainChange = true; 00015 set( b0/a0, b1/a0, b2/a0, a1/a0, a2/a0 ); 00016 } 00017 00018 void BiQuad::PIDF( double Kp, double Ki, double Kd, double N, double Ts ) { 00019 00020 double b0, b1, b2, bd, a1, a2; 00021 00022 a1 = -4.0/(N*Ts+2.0); 00023 a2 = -(N*Ts-2.0)/(N*Ts+2.0); 00024 00025 bd = ( N*Ts+2.0 ); 00026 00027 b0 = ( 4.0*Kp + 4.0*Kd*N + 2.0*Ki*Ts + 2.0*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd); 00028 b1 = ( Ki*N*Ts*Ts - 4.0*Kp - 4.0*Kd*N )/bd; 00029 b2 = ( 4.0*Kp + 4.0*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd); 00030 00031 set( b0, b1, b2, a1, a2 ); 00032 00033 }; 00034 00035 void BiQuad::set(double b0, double b1, double b2, double a1, double a2) { 00036 00037 B[0] = b0; B[1] = b1; B[2] = b2; 00038 A[0] = a1; A[1] = a2; 00039 00040 if( resetStateOnGainChange ) 00041 wz[0] = 0; wz[1] = 0; 00042 00043 } 00044 00045 double BiQuad::step(double x) { 00046 00047 double y,w; 00048 00049 /* Direct form II */ 00050 w = x - A[0]*wz[0] - A[1]*wz[1]; 00051 y = B[0]*w + B[1]*wz[0] + B[2]*wz[1]; 00052 00053 /* Shift */ 00054 wz[1] = wz[0]; 00055 wz[0] = w; 00056 00057 return y; 00058 00059 } 00060 00061 std::vector< std::complex<double> > BiQuad::poles() { 00062 00063 std::vector< std::complex<double> > poles; 00064 00065 std::complex<double> b2(A[0]*A[0],0); 00066 std::complex<double> ds = std::sqrt( b2-4*A[1] ); 00067 00068 poles.push_back( 0.5*(-A[0]+ds) ); 00069 poles.push_back( 0.5*(-A[0]-ds) ); 00070 00071 return poles; 00072 00073 } 00074 00075 std::vector< std::complex<double> > BiQuad::zeros() { 00076 00077 std::vector< std::complex<double> > zeros; 00078 00079 std::complex<double> b2(B[1]*B[1],0); 00080 std::complex<double> ds = std::sqrt( b2-4*B[0]*B[2] ); 00081 00082 zeros.push_back( 0.5*(-B[1]+ds)/B[0] ); 00083 zeros.push_back( 0.5*(-B[1]-ds)/B[0] ); 00084 00085 return zeros; 00086 00087 } 00088 00089 bool BiQuad::stable() { 00090 bool stable = true; 00091 std::vector< std::complex<double> > ps = poles(); 00092 for( size_t i = 0; i < ps.size(); i++ ) 00093 stable = stable & ( std::abs( ps[i] ) < 1 ); 00094 return stable; 00095 } 00096 00097 void BiQuad::setResetStateOnGainChange( bool v ){ 00098 resetStateOnGainChange = v; 00099 } 00100 00101 BiQuadChain &BiQuadChain::add(BiQuad *bq) { 00102 biquads.push_back( bq ); 00103 return *this; 00104 } 00105 00106 BiQuadChain operator*( BiQuad &bq1, BiQuad &bq2 ) { 00107 BiQuadChain bqc; 00108 bqc.add( &bq1 ).add( &bq2 ); 00109 return bqc; 00110 } 00111 00112 double BiQuadChain::step(double x) { 00113 00114 int i; 00115 size_t bqs; 00116 00117 bqs = biquads.size(); 00118 00119 for( i = 0; i < bqs; i++ ) 00120 x = biquads[i]->step( x ); 00121 00122 return x; 00123 } 00124 00125 std::vector< std::complex<double> > BiQuadChain::poles_zeros( bool zeros ) { 00126 00127 std::vector< std::complex<double> > chain, bq; 00128 int i; 00129 size_t bqs; 00130 00131 bqs = biquads.size(); 00132 00133 for( i = 0; i < bqs; i++ ){ 00134 bq = ( zeros ) ? biquads[ i ]->zeros() : biquads[ i ]->poles(); 00135 chain.insert( chain.end(), bq.begin(), bq.end() ); 00136 } 00137 00138 return chain; 00139 00140 } 00141 00142 std::vector< std::complex<double> > BiQuadChain::poles() { 00143 return poles_zeros( false ); 00144 } 00145 00146 std::vector< std::complex<double> > BiQuadChain::zeros() { 00147 return poles_zeros( true ); 00148 } 00149 00150 bool BiQuadChain::stable() { 00151 bool stable = true; 00152 for( size_t i = 0; i < biquads.size(); i++ ) 00153 stable = stable & biquads[i]->stable(); 00154 return stable; 00155 } 00156 00157 BiQuadChain& BiQuadChain::operator*( BiQuad& bq ) { 00158 add( &bq ); 00159 return *this; 00160 }
Generated on Thu Jul 14 2022 07:24:36 by 1.7.2