speedestimator.h

00001 #ifndef SPEEDESTIMATOR_H
00002 #define SPEEDESTIMATOR_H
00003 
00004 #include "main.h"
00005 
00006 struct speedEstimator {
00007 
00008     int last_position;
00009     float position_int;
00010     int dead_ticks;
00011 
00012     static const int fast_gain = SPEED_ESTIMATOR_FAST_GAIN;
00013     static const int deadband = SPEED_ESTIMATOR_DEADBAND;
00014 
00015     /**
00016      * constructor
00017      *
00018      * Sets the default properties of the struct
00019      */
00020     speedEstimator(int position) {
00021 
00022         // Set initial positions
00023         last_position = position;
00024         position_int = 1.0f*position;
00025         dead_ticks = 1;
00026     };
00027 
00028     float get(int position) {
00029 
00030         /**
00031          * Slow estimation
00032          */
00033         static float yz1 = 0.0f;
00034         static float xz0 = 0.0f;
00035         static float xz1 = 0.0f;
00036         static float dp  = 0;
00037         static float dp_dead = 0;
00038 
00039         dp = position - last_position;
00040 
00041         if(dp==0) {
00042             if(dead_ticks < 10000)
00043                 dead_ticks++;
00044             dp = dp_dead/dead_ticks;
00045         } else {
00046             if(dp > POSITION_ANTIALIAS)
00047                 dp-=POSITION_RESOLUTION;
00048             else if(dp < -POSITION_ANTIALIAS)
00049                 dp+=POSITION_RESOLUTION;
00050 
00051             dp = dp/dead_ticks;
00052             dp_dead = dp;
00053             dead_ticks = 1;
00054         }
00055 
00056         xz0 = SAMPLE_TIME_INVERSE_S*dp;
00057         yz1 = SPEED_ESTIMATOR_FILTER * xz1 + (1-SPEED_ESTIMATOR_FILTER) * yz1;
00058         xz1 = xz0;
00059 
00060         last_position = position;
00061 
00062         /**
00063          * Fast estimation
00064          */
00065         static float fast_sum;
00066         static float pre_int_sum;
00067 
00068         fast_sum = position - (position_int + SAMPLE_TIME_US*0.5f*(float)1e-6*yz1);
00069         pre_int_sum = yz1+speedEstimator::fast_gain*(fast_sum > speedEstimator::deadband ? fast_sum - speedEstimator::deadband : (fast_sum < -speedEstimator::deadband ? fast_sum + speedEstimator::deadband : 0));
00070         position_int+=SAMPLE_TIME_US*1e-6*pre_int_sum;
00071 
00072         return pre_int_sum;
00073 
00074     };
00075 
00076 };
00077 
00078 #endif