Meike Froklage / Mbed 2 deprecated The_Claw_kloten

The code of the claw with filter

Dependencies:   MODSERIAL Motordriver QEI Servo mbed HIDScope

Fork of The_Claw by Meike Froklage

BiQuad.cpp@7:defb5001b787, 2016-11-01 (annotated)

Committer:
meikefrok
Date:
Tue Nov 01 13:02:00 2016 +0000
Revision:
7:defb5001b787
Parent:
6:803234ba7071 
New restrictions;

Who changed what in which revision?

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