Biorobotics_group_2 / Mbed 2 deprecated Encoder

Reads the encoder signals from 1 motor and displays the counts and degrees of the output shaft in Serial.

Dependencies:   HIDScope_motor_ff QEI mbed FastPWM MODSERIAL

Fork of HID_scope_test by Biorobotics_group_2

BiQuad.cpp@9:278d25dc0ef3, 2016-10-28 (annotated)

Committer:
sjoerdbarts
Date:
Fri Oct 28 09:58:22 2016 +0000
Revision:
9:278d25dc0ef3
Parent:
8:fe907b9a0bab 
Working code

Who changed what in which revision?

UserRevisionLine numberNew 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 }