Biorobotics_group_2 / Mbed 2 deprecated Berekenen_motorhoek

Diagonaal berekenen lukt, meerdere outputs nog niet

Dependencies:   mbed FastPWM HIDScope MODSERIAL QEI

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