FSG / PosVelFilter

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.

Fork of PosVelFilter_7_14 by Troy Holley

PosVelFilter.cpp@0:82f27e7e99f0, 2017-04-27 (annotated)

Committer:
tzyoung
Date:
Thu Apr 27 13:15:55 2017 +0000
Revision:
0:82f27e7e99f0
Child:
1:ec4117689673
initial release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tzyoung 0:82f27e7e99f0 1 #include "PosVelFilter.hpp"
tzyoung 0:82f27e7e99f0 2 #include "StaticDefs.hpp"
tzyoung 0:82f27e7e99f0 3 #include "conversions.hpp"
tzyoung 0:82f27e7e99f0 4
tzyoung 0:82f27e7e99f0 5 PosVelFilter::PosVelFilter()
tzyoung 0:82f27e7e99f0 6 {
tzyoung 0:82f27e7e99f0 7 x1 = 0;
tzyoung 0:82f27e7e99f0 8 x2 = 0;
tzyoung 0:82f27e7e99f0 9 //w_n is the natural frequency of the filter bigger increases frequency response
tzyoung 0:82f27e7e99f0 10 w_n = 5.0;
tzyoung 0:82f27e7e99f0 11
tzyoung 0:82f27e7e99f0 12 }
tzyoung 0:82f27e7e99f0 13
tzyoung 0:82f27e7e99f0 14 void PosVelFilter::update()
tzyoung 0:82f27e7e99f0 15 {
tzyoung 0:82f27e7e99f0 16 //run the pos/vel estimate filter
tzyoung 0:82f27e7e99f0 17 //this derives the timing from last run
tzyoung 0:82f27e7e99f0 18 last_time = curr_time;
tzyoung 0:82f27e7e99f0 19 curr_time = systemTime().read();
tzyoung 0:82f27e7e99f0 20 dt = curr_time-last_time;
tzyoung 0:82f27e7e99f0 21
tzyoung 0:82f27e7e99f0 22 // fetch the current distance reading from adc and convert to mm
tzyoung 0:82f27e7e99f0 23 conv_distance = counts_to_dist(adc().ch0_filt);
tzyoung 0:82f27e7e99f0 24
tzyoung 0:82f27e7e99f0 25 x1_dot = x2;
tzyoung 0:82f27e7e99f0 26 x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*conv_distance;
tzyoung 0:82f27e7e99f0 27
tzyoung 0:82f27e7e99f0 28 position = x1;
tzyoung 0:82f27e7e99f0 29 velocity = x2;
tzyoung 0:82f27e7e99f0 30
tzyoung 0:82f27e7e99f0 31 x1 += x1_dot*dt;
tzyoung 0:82f27e7e99f0 32 x2 += x2_dot*dt;
tzyoung 0:82f27e7e99f0 33
tzyoung 0:82f27e7e99f0 34 }
tzyoung 0:82f27e7e99f0 35
tzyoung 0:82f27e7e99f0 36 void PosVelFilter::init()
tzyoung 0:82f27e7e99f0 37 {
tzyoung 0:82f27e7e99f0 38 // run filter for 2 seconds on startup
tzyoung 0:82f27e7e99f0 39 float start = systemTime().read();
tzyoung 0:82f27e7e99f0 40 float time = start;
tzyoung 0:82f27e7e99f0 41 pc().printf("\n\rWarming Up \n\r");
tzyoung 0:82f27e7e99f0 42
tzyoung 0:82f27e7e99f0 43 while ((time - start) < 1.0) {
tzyoung 0:82f27e7e99f0 44 update();
tzyoung 0:82f27e7e99f0 45
tzyoung 0:82f27e7e99f0 46 pc().printf("%5.3f \r", velocity);
tzyoung 0:82f27e7e99f0 47 time = systemTime().read();
tzyoung 0:82f27e7e99f0 48 }
tzyoung 0:82f27e7e99f0 49 pc().printf("\n\r");
tzyoung 0:82f27e7e99f0 50 while (1) {
tzyoung 0:82f27e7e99f0 51 update();
tzyoung 0:82f27e7e99f0 52
tzyoung 0:82f27e7e99f0 53 if (abs(velocity) > 0.005) {
tzyoung 0:82f27e7e99f0 54 pc().printf("Waiting to stablize. velocity: %5.3f \r", velocity);
tzyoung 0:82f27e7e99f0 55
tzyoung 0:82f27e7e99f0 56 } else {
tzyoung 0:82f27e7e99f0 57 pc().printf("\n\rreading stabilized\n\r");
tzyoung 0:82f27e7e99f0 58 break;
tzyoung 0:82f27e7e99f0 59 }
tzyoung 0:82f27e7e99f0 60 }
tzyoung 0:82f27e7e99f0 61
tzyoung 0:82f27e7e99f0 62 }
tzyoung 0:82f27e7e99f0 63
tzyoung 0:82f27e7e99f0 64
tzyoung 0:82f27e7e99f0 65 float PosVelFilter::getPosition()
tzyoung 0:82f27e7e99f0 66 {
tzyoung 0:82f27e7e99f0 67 return position;
tzyoung 0:82f27e7e99f0 68 }
tzyoung 0:82f27e7e99f0 69
tzyoung 0:82f27e7e99f0 70 float PosVelFilter::getVelocity()
tzyoung 0:82f27e7e99f0 71 {
tzyoung 0:82f27e7e99f0 72 return velocity;
tzyoung 0:82f27e7e99f0 73 }
tzyoung 0:82f27e7e99f0 74
tzyoung 0:82f27e7e99f0 75 float PosVelFilter::getDt()
tzyoung 0:82f27e7e99f0 76 {
tzyoung 0:82f27e7e99f0 77 return dt;
tzyoung 0:82f27e7e99f0 78 }
tzyoung 0:82f27e7e99f0 79
tzyoung 0:82f27e7e99f0 80 void PosVelFilter::writeWn(float wn)
tzyoung 0:82f27e7e99f0 81 {
tzyoung 0:82f27e7e99f0 82 w_n = wn;
tzyoung 0:82f27e7e99f0 83 pc().printf("\n\rWn set to: %f", w_n);
tzyoung 0:82f27e7e99f0 84 }