Shimon AJISAKA / Mbed 2 deprecated InvertedPendulum2017

2-wheel Inverted Pendulum control program

Dependencies:   AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed

filter_func.cpp@6:a5f674c2f262, 2018-04-20 (annotated)

Committer:
bluefish
Date:
Fri Apr 20 18:14:15 2018 +0000
Revision:
6:a5f674c2f262
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bluefish 6:a5f674c2f262 1 #include "filter_func.h"
bluefish 6:a5f674c2f262 2
bluefish 6:a5f674c2f262 3 // initialize complimentary filter
bluefish 6:a5f674c2f262 4 void init_comp_filter( STCOMPFILTER* filter, float param ){
bluefish 6:a5f674c2f262 5 filter->_int = 0.0f;
bluefish 6:a5f674c2f262 6 filter->_adj = 0.0f;
bluefish 6:a5f674c2f262 7 filter->_adj_temp = 0.0f;
bluefish 6:a5f674c2f262 8 filter->_param = param;
bluefish 6:a5f674c2f262 9 return;
bluefish 6:a5f674c2f262 10 }
bluefish 6:a5f674c2f262 11
bluefish 6:a5f674c2f262 12 // process complimentary filter
bluefish 6:a5f674c2f262 13 void proc_comp_filter( STCOMPFILTER* filter, float dt, float angle_raw, float angle_vec_raw, float* angle, float* angle_vec ){
bluefish 6:a5f674c2f262 14 if( filter->_param == 0.0f ){ return; }
bluefish 6:a5f674c2f262 15 filter->_adj_temp = filter->_adj;
bluefish 6:a5f674c2f262 16 filter->_int += angle_vec_raw * dt; // integral gyro
bluefish 6:a5f674c2f262 17 filter->_adj = angle_raw - filter->_int; //
bluefish 6:a5f674c2f262 18 filter->_adj = filter->_adj_temp + dt * ( filter->_adj - filter->_adj_temp ) / filter->_param; // calc correction via LPF
bluefish 6:a5f674c2f262 19
bluefish 6:a5f674c2f262 20 *angle = filter->_int + filter->_adj;
bluefish 6:a5f674c2f262 21 *angle_vec = angle_vec_raw;
bluefish 6:a5f674c2f262 22 return;
bluefish 6:a5f674c2f262 23 }
bluefish 6:a5f674c2f262 24
bluefish 6:a5f674c2f262 25 // initialize BiQuad filter
bluefish 6:a5f674c2f262 26 void init_biquad_filter( STBIQUADFILTER* filter, float freq_sample, float freq, float param, BIQUADTYPE filter_pattern ){
bluefish 6:a5f674c2f262 27 float omega;
bluefish 6:a5f674c2f262 28 float alpha;
bluefish 6:a5f674c2f262 29
bluefish 6:a5f674c2f262 30 if( freq_sample == 0.0f ){ return; }
bluefish 6:a5f674c2f262 31 omega = 2.0f * 3.14159265f * freq / freq_sample;
bluefish 6:a5f674c2f262 32
bluefish 6:a5f674c2f262 33 switch( filter_pattern ){
bluefish 6:a5f674c2f262 34 case FILTER_LPF:
bluefish 6:a5f674c2f262 35 alpha = sinf(omega)/(2.0f*param);
bluefish 6:a5f674c2f262 36 filter->b[0] = (1.0f - cos(omega))/2.0f;
bluefish 6:a5f674c2f262 37 filter->b[1] = 1.0f - cos(omega);
bluefish 6:a5f674c2f262 38 filter->b[2] = (1.0f - cos(omega))/2.0f;
bluefish 6:a5f674c2f262 39 break;
bluefish 6:a5f674c2f262 40 case FILTER_HPF:
bluefish 6:a5f674c2f262 41 alpha = sinf(omega)/(2.0f*param);
bluefish 6:a5f674c2f262 42 filter->b[0] = (1.0f + cos(omega))/2.0f;
bluefish 6:a5f674c2f262 43 filter->b[1] = -1.0f - cos(omega);
bluefish 6:a5f674c2f262 44 filter->b[2] = (1.0f + cos(omega))/2.0f;
bluefish 6:a5f674c2f262 45 break;
bluefish 6:a5f674c2f262 46 case FILTER_BPF:
bluefish 6:a5f674c2f262 47 alpha = sinf(omega)*sinhf( 0.15051500f * param * omega / sinf(omega) );
bluefish 6:a5f674c2f262 48 filter->b[0] = alpha;
bluefish 6:a5f674c2f262 49 filter->b[1] = 0.0f;
bluefish 6:a5f674c2f262 50 filter->b[2] = -alpha;
bluefish 6:a5f674c2f262 51 break;
bluefish 6:a5f674c2f262 52 case FILTER_NF:
bluefish 6:a5f674c2f262 53 alpha = sinf(omega)*sinhf( 0.15051500f * param * omega / sinf(omega) );
bluefish 6:a5f674c2f262 54 filter->b[0] = 1.0f;
bluefish 6:a5f674c2f262 55 filter->b[1] = -2.0f * cosf(omega);
bluefish 6:a5f674c2f262 56 filter->b[2] = 1.0f;
bluefish 6:a5f674c2f262 57 break;
bluefish 6:a5f674c2f262 58 default:
bluefish 6:a5f674c2f262 59 break;
bluefish 6:a5f674c2f262 60 }
bluefish 6:a5f674c2f262 61 filter->a[0] = 1.0f + alpha;
bluefish 6:a5f674c2f262 62 filter->a[1] = -2.0f * cosf(omega);
bluefish 6:a5f674c2f262 63 filter->a[2] = 1.0f - alpha;
bluefish 6:a5f674c2f262 64
bluefish 6:a5f674c2f262 65 filter->in[0] = 0.0f;
bluefish 6:a5f674c2f262 66 filter->in[1] = 0.0f;
bluefish 6:a5f674c2f262 67 filter->out[0] = 0.0f;
bluefish 6:a5f674c2f262 68 filter->out[1] = 0.0f;
bluefish 6:a5f674c2f262 69
bluefish 6:a5f674c2f262 70 return;
bluefish 6:a5f674c2f262 71 }
bluefish 6:a5f674c2f262 72
bluefish 6:a5f674c2f262 73 // process BiQuad Filter
bluefish 6:a5f674c2f262 74 float proc_biquad_filter( STBIQUADFILTER* filter, float input ){
bluefish 6:a5f674c2f262 75 float output;
bluefish 6:a5f674c2f262 76 if( filter->a[0] == 0.0f ){ return 0.0f; }
bluefish 6:a5f674c2f262 77
bluefish 6:a5f674c2f262 78 output =
bluefish 6:a5f674c2f262 79 (filter->b[0]/filter->a[0]) * input
bluefish 6:a5f674c2f262 80 + (filter->b[1]/filter->a[0] )* filter->in[0]
bluefish 6:a5f674c2f262 81 + (filter->b[2]/filter->a[0]) * filter->in[1]
bluefish 6:a5f674c2f262 82 - (filter->a[1]/filter->a[0]) * filter->out[0]
bluefish 6:a5f674c2f262 83 - (filter->a[2]/filter->a[0]) * filter->out[1];
bluefish 6:a5f674c2f262 84
bluefish 6:a5f674c2f262 85 filter->in[1] = filter->in[0];
bluefish 6:a5f674c2f262 86 filter->in[0] = input;
bluefish 6:a5f674c2f262 87
bluefish 6:a5f674c2f262 88 filter->out[1] = filter->out[0];
bluefish 6:a5f674c2f262 89 filter->out[0] = output;
bluefish 6:a5f674c2f262 90
bluefish 6:a5f674c2f262 91 return output;
bluefish 6:a5f674c2f262 92 }