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@1:24b7ab90081a, 2015-01-17 (annotated)

Committer:
vsluiter
Date:
Sat Jan 17 21:42:46 2015 +0000
Revision:
1:24b7ab90081a
Parent:
0:f3cf9865b7be 
Added state machine for impedances

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tomlankhorst 0:f3cf9865b7be 1 #ifndef SPEEDESTIMATOR_H
tomlankhorst 0:f3cf9865b7be 2 #define SPEEDESTIMATOR_H
tomlankhorst 0:f3cf9865b7be 3
tomlankhorst 0:f3cf9865b7be 4 #include "main.h"
tomlankhorst 0:f3cf9865b7be 5
tomlankhorst 0:f3cf9865b7be 6 struct speedEstimator {
vsluiter 1:24b7ab90081a 7
tomlankhorst 0:f3cf9865b7be 8 int last_position;
tomlankhorst 0:f3cf9865b7be 9 float position_int;
tomlankhorst 0:f3cf9865b7be 10 int dead_ticks;
vsluiter 1:24b7ab90081a 11
tomlankhorst 0:f3cf9865b7be 12 static const int fast_gain = SPEED_ESTIMATOR_FAST_GAIN;
tomlankhorst 0:f3cf9865b7be 13 static const int deadband = SPEED_ESTIMATOR_DEADBAND;
vsluiter 1:24b7ab90081a 14
tomlankhorst 0:f3cf9865b7be 15 /**
tomlankhorst 0:f3cf9865b7be 16 * constructor
vsluiter 1:24b7ab90081a 17 *
tomlankhorst 0:f3cf9865b7be 18 * Sets the default properties of the struct
tomlankhorst 0:f3cf9865b7be 19 */
vsluiter 1:24b7ab90081a 20 speedEstimator(int position) {
vsluiter 1:24b7ab90081a 21
tomlankhorst 0:f3cf9865b7be 22 // Set initial positions
tomlankhorst 0:f3cf9865b7be 23 last_position = position;
tomlankhorst 0:f3cf9865b7be 24 position_int = 1.0f*position;
tomlankhorst 0:f3cf9865b7be 25 dead_ticks = 1;
tomlankhorst 0:f3cf9865b7be 26 };
vsluiter 1:24b7ab90081a 27
tomlankhorst 0:f3cf9865b7be 28 float get(int position) {
vsluiter 1:24b7ab90081a 29
tomlankhorst 0:f3cf9865b7be 30 /**
tomlankhorst 0:f3cf9865b7be 31 * Slow estimation
tomlankhorst 0:f3cf9865b7be 32 */
tomlankhorst 0:f3cf9865b7be 33 static float yz1 = 0.0f;
tomlankhorst 0:f3cf9865b7be 34 static float xz0 = 0.0f;
tomlankhorst 0:f3cf9865b7be 35 static float xz1 = 0.0f;
tomlankhorst 0:f3cf9865b7be 36 static float dp = 0;
tomlankhorst 0:f3cf9865b7be 37 static float dp_dead = 0;
tomlankhorst 0:f3cf9865b7be 38
tomlankhorst 0:f3cf9865b7be 39 dp = position - last_position;
vsluiter 1:24b7ab90081a 40
vsluiter 1:24b7ab90081a 41 if(dp==0) {
tomlankhorst 0:f3cf9865b7be 42 if(dead_ticks < 10000)
vsluiter 1:24b7ab90081a 43 dead_ticks++;
tomlankhorst 0:f3cf9865b7be 44 dp = dp_dead/dead_ticks;
tomlankhorst 0:f3cf9865b7be 45 } else {
tomlankhorst 0:f3cf9865b7be 46 if(dp > POSITION_ANTIALIAS)
tomlankhorst 0:f3cf9865b7be 47 dp-=POSITION_RESOLUTION;
tomlankhorst 0:f3cf9865b7be 48 else if(dp < -POSITION_ANTIALIAS)
tomlankhorst 0:f3cf9865b7be 49 dp+=POSITION_RESOLUTION;
vsluiter 1:24b7ab90081a 50
tomlankhorst 0:f3cf9865b7be 51 dp = dp/dead_ticks;
tomlankhorst 0:f3cf9865b7be 52 dp_dead = dp;
tomlankhorst 0:f3cf9865b7be 53 dead_ticks = 1;
tomlankhorst 0:f3cf9865b7be 54 }
vsluiter 1:24b7ab90081a 55
tomlankhorst 0:f3cf9865b7be 56 xz0 = SAMPLE_TIME_INVERSE_S*dp;
tomlankhorst 0:f3cf9865b7be 57 yz1 = SPEED_ESTIMATOR_FILTER * xz1 + (1-SPEED_ESTIMATOR_FILTER) * yz1;
tomlankhorst 0:f3cf9865b7be 58 xz1 = xz0;
vsluiter 1:24b7ab90081a 59
tomlankhorst 0:f3cf9865b7be 60 last_position = position;
vsluiter 1:24b7ab90081a 61
tomlankhorst 0:f3cf9865b7be 62 /**
tomlankhorst 0:f3cf9865b7be 63 * Fast estimation
tomlankhorst 0:f3cf9865b7be 64 */
tomlankhorst 0:f3cf9865b7be 65 static float fast_sum;
tomlankhorst 0:f3cf9865b7be 66 static float pre_int_sum;
vsluiter 1:24b7ab90081a 67
tomlankhorst 0:f3cf9865b7be 68 fast_sum = position - (position_int + SAMPLE_TIME_US*0.5f*(float)1e-6*yz1);
tomlankhorst 0:f3cf9865b7be 69 pre_int_sum = yz1+speedEstimator::fast_gain*(fast_sum > speedEstimator::deadband ? fast_sum - speedEstimator::deadband : (fast_sum < -speedEstimator::deadband ? fast_sum + speedEstimator::deadband : 0));
tomlankhorst 0:f3cf9865b7be 70 position_int+=SAMPLE_TIME_US*1e-6*pre_int_sum;
vsluiter 1:24b7ab90081a 71
vsluiter 1:24b7ab90081a 72 return pre_int_sum;
vsluiter 1:24b7ab90081a 73
tomlankhorst 0:f3cf9865b7be 74 };
vsluiter 1:24b7ab90081a 75
tomlankhorst 0:f3cf9865b7be 76 };
tomlankhorst 0:f3cf9865b7be 77
tomlankhorst 0:f3cf9865b7be 78 #endif