Poep Hoofd / Mbed 2 deprecated PoolRobot_Code

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope mbed MODSERIAL QEI

BiQuad.cpp@21:59431788a42d, 2017-11-06 (annotated)

Committer:
john111222333
Date:
Mon Nov 06 09:57:21 2017 +0000
Revision:
21:59431788a42d
Parent:
0:2d9dae739559 
final version;

Who changed what in which revision?

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