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