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