Joris Kaal / Mbed 2 deprecated haptic_hid_1s_PRBS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL USBDevice compensation_tables mbed-dsp mbed

Fork of haptic_hid by First Last

speedestimator.h

Committer:
tomlankhorst
Date:
2015-01-16
Revision:
0:f3cf9865b7be
Child:
1:24b7ab90081a

File content as of revision 0:f3cf9865b7be:

#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