Diff: speedestimator.h

Revision:

0:f3cf9865b7be

Child:

1:24b7ab90081a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/speedestimator.h	Fri Jan 16 10:47:17 2015 +0000
@@ -0,0 +1,78 @@
+#ifndef SPEEDESTIMATOR_H
+#define SPEEDESTIMATOR_H
+
+#include "main.h"
+
+struct speedEstimator {
+    
+    int last_position;
+    float position_int;
+    int dead_ticks;
+    
+    static const int fast_gain = SPEED_ESTIMATOR_FAST_GAIN;
+    static const int deadband = SPEED_ESTIMATOR_DEADBAND;
+    
+    /**
+     * constructor
+     * 
+     * Sets the default properties of the struct
+     */
+    speedEstimator(int position) { 
+    
+        // Set initial positions
+        last_position = position;
+        position_int = 1.0f*position;
+        dead_ticks = 1;
+    };
+    
+    float get(int position) {
+        
+        /**
+         * Slow estimation
+         */
+        static float yz1 = 0.0f;
+        static float xz0 = 0.0f;
+        static float xz1 = 0.0f;
+        static float dp  = 0;
+        static float dp_dead = 0;
+
+        dp = position - last_position;
+        
+        if(dp==0){
+            if(dead_ticks < 10000)
+                dead_ticks++; 
+            dp = dp_dead/dead_ticks;
+        } else {
+            if(dp > POSITION_ANTIALIAS)
+                dp-=POSITION_RESOLUTION;
+            else if(dp < -POSITION_ANTIALIAS)
+                dp+=POSITION_RESOLUTION;
+            
+            dp = dp/dead_ticks;
+            dp_dead = dp;
+            dead_ticks = 1;
+        }
+            
+        xz0 = SAMPLE_TIME_INVERSE_S*dp;
+        yz1 = SPEED_ESTIMATOR_FILTER * xz1 + (1-SPEED_ESTIMATOR_FILTER) * yz1;
+        xz1 = xz0;
+        
+        last_position = position;
+        
+        /**
+         * Fast estimation
+         */
+        static float fast_sum;
+        static float pre_int_sum;
+        
+        fast_sum = position - (position_int + SAMPLE_TIME_US*0.5f*(float)1e-6*yz1);
+        pre_int_sum = yz1+speedEstimator::fast_gain*(fast_sum > speedEstimator::deadband ? fast_sum - speedEstimator::deadband : (fast_sum < -speedEstimator::deadband ? fast_sum + speedEstimator::deadband : 0));
+        position_int+=SAMPLE_TIME_US*1e-6*pre_int_sum;
+    
+        return pre_int_sum;    
+        
+    };
+    
+};
+
+#endif
\ No newline at end of file