Shimon AJISAKA / Mbed 2 deprecated InvertedPendulum2017

2-wheel Inverted Pendulum control program

Dependencies:   AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed

filter_func.h@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 #ifndef __FILTER_FUNCTIONS_H__
bluefish 6:a5f674c2f262 2 #define __FILTER_FUNCTIONS_H__
bluefish 6:a5f674c2f262 3
bluefish 6:a5f674c2f262 4 #include <math.h>
bluefish 6:a5f674c2f262 5 #include "mbed.h"
bluefish 6:a5f674c2f262 6
bluefish 6:a5f674c2f262 7 typedef enum _BIQUADTYPE{
bluefish 6:a5f674c2f262 8 FILTER_LPF = 1,
bluefish 6:a5f674c2f262 9 FILTER_HPF = 2,
bluefish 6:a5f674c2f262 10 FILTER_BPF = 3,
bluefish 6:a5f674c2f262 11 FILTER_NF = 4
bluefish 6:a5f674c2f262 12 } BIQUADTYPE;
bluefish 6:a5f674c2f262 13
bluefish 6:a5f674c2f262 14 typedef struct _STCOMPFILTER{
bluefish 6:a5f674c2f262 15 float _int;
bluefish 6:a5f674c2f262 16 float _adj;
bluefish 6:a5f674c2f262 17 float _adj_temp;
bluefish 6:a5f674c2f262 18 float _param;
bluefish 6:a5f674c2f262 19 } STCOMPFILTER;
bluefish 6:a5f674c2f262 20
bluefish 6:a5f674c2f262 21 typedef struct _STBIQUADFILTER{
bluefish 6:a5f674c2f262 22 float in[2]; // input
bluefish 6:a5f674c2f262 23 float out[2]; // output
bluefish 6:a5f674c2f262 24 float a[3]; // coefficient A
bluefish 6:a5f674c2f262 25 float b[3]; // coefficient B
bluefish 6:a5f674c2f262 26 } STBIQUADFILTER;
bluefish 6:a5f674c2f262 27
bluefish 6:a5f674c2f262 28 // complimentary filter
bluefish 6:a5f674c2f262 29 void init_comp_filter( STCOMPFILTER* filter, float param );
bluefish 6:a5f674c2f262 30 void proc_comp_filter( STCOMPFILTER* filter, float dt, float angle_raw, float angle_vec_raw, float* angle, float* angle_vec );
bluefish 6:a5f674c2f262 31
bluefish 6:a5f674c2f262 32 // BiQuad filter
bluefish 6:a5f674c2f262 33 void init_biquad_filter( STBIQUADFILTER* filter, float freq_sample, float freq, float param, BIQUADTYPE filter_pattern );
bluefish 6:a5f674c2f262 34 float proc_biquad_filter( STBIQUADFILTER* filter, float input );
bluefish 6:a5f674c2f262 35
bluefish 6:a5f674c2f262 36 #endif