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@3:8107bb13278b, 2017-07-28 (annotated)
- Committer:
- mdavis30
- Date:
- Fri Jul 28 18:49:50 2017 +0000
- Revision:
- 3:8107bb13278b
- Parent:
- 2:992e774dc62a
Who changed what in which revision?
| User | Revision | Line number | New 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 |
| tnhnrl | 2:992e774dc62a | 10 | |
| tnhnrl | 2:992e774dc62a | 11 | //w_n = 5.0; // larger number equals faster response |
| tnhnrl | 2:992e774dc62a | 12 | |
| tnhnrl | 2:992e774dc62a | 13 | w_n = 2.0; //5.0 was way off |
| tzyoung | 0:82f27e7e99f0 | 14 | |
| tzyoung | 0:82f27e7e99f0 | 15 | } |
| tzyoung | 0:82f27e7e99f0 | 16 | |
| tzyoung | 0:82f27e7e99f0 | 17 | void PosVelFilter::update() |
| tzyoung | 0:82f27e7e99f0 | 18 | { |
| tzyoung | 0:82f27e7e99f0 | 19 | //run the pos/vel estimate filter |
| tzyoung | 0:82f27e7e99f0 | 20 | //this derives the timing from last run |
| tzyoung | 0:82f27e7e99f0 | 21 | last_time = curr_time; |
| tzyoung | 0:82f27e7e99f0 | 22 | curr_time = systemTime().read(); |
| tzyoung | 0:82f27e7e99f0 | 23 | dt = curr_time-last_time; |
| tzyoung | 0:82f27e7e99f0 | 24 | |
| tzyoung | 0:82f27e7e99f0 | 25 | // fetch the current distance reading from adc and convert to mm |
| tzyoung | 0:82f27e7e99f0 | 26 | conv_distance = counts_to_dist(adc().ch0_filt); |
| tnhnrl | 2:992e774dc62a | 27 | //conv_distance = counts_to_dist(300); //testing |
| tzyoung | 0:82f27e7e99f0 | 28 | |
| tnhnrl | 2:992e774dc62a | 29 | x1_dot = x2*1.0; |
| tzyoung | 0:82f27e7e99f0 | 30 | x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*conv_distance; |
| tzyoung | 0:82f27e7e99f0 | 31 | |
| tzyoung | 0:82f27e7e99f0 | 32 | position = x1; |
| tzyoung | 0:82f27e7e99f0 | 33 | velocity = x2; |
| tnhnrl | 2:992e774dc62a | 34 | |
| tnhnrl | 2:992e774dc62a | 35 | //JUST A TEST (TROY) |
| tnhnrl | 2:992e774dc62a | 36 | //position = dt; |
| tnhnrl | 2:992e774dc62a | 37 | //velocity = 0; //velocity needs to be zero, stabilize |
| tnhnrl | 2:992e774dc62a | 38 | //conv_distance = 1234; |
| tnhnrl | 2:992e774dc62a | 39 | |
| tzyoung | 0:82f27e7e99f0 | 40 | |
| tzyoung | 0:82f27e7e99f0 | 41 | x1 += x1_dot*dt; |
| tzyoung | 0:82f27e7e99f0 | 42 | x2 += x2_dot*dt; |
| tzyoung | 0:82f27e7e99f0 | 43 | |
| tzyoung | 0:82f27e7e99f0 | 44 | } |
| tzyoung | 0:82f27e7e99f0 | 45 | |
| mdavis30 | 3:8107bb13278b | 46 | void PosVelFilter::update_la() |
| mdavis30 | 3:8107bb13278b | 47 | { |
| mdavis30 | 3:8107bb13278b | 48 | //run the pos/vel estimate filter |
| mdavis30 | 3:8107bb13278b | 49 | //this derives the timing from last run |
| mdavis30 | 3:8107bb13278b | 50 | last_time = curr_time; |
| mdavis30 | 3:8107bb13278b | 51 | curr_time = systemTime().read(); |
| mdavis30 | 3:8107bb13278b | 52 | dt = curr_time-last_time; |
| mdavis30 | 3:8107bb13278b | 53 | |
| mdavis30 | 3:8107bb13278b | 54 | // fetch the current distance reading from adc and convert to mm |
| mdavis30 | 3:8107bb13278b | 55 | conv_distance = counts_to_dist(adc().ch1_filt); |
| mdavis30 | 3:8107bb13278b | 56 | //conv_distance = counts_to_dist(300); //testing |
| mdavis30 | 3:8107bb13278b | 57 | |
| mdavis30 | 3:8107bb13278b | 58 | x1_dot = x2*1.0; |
| mdavis30 | 3:8107bb13278b | 59 | x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*conv_distance; |
| mdavis30 | 3:8107bb13278b | 60 | |
| mdavis30 | 3:8107bb13278b | 61 | position = x1; |
| mdavis30 | 3:8107bb13278b | 62 | velocity = x2; |
| mdavis30 | 3:8107bb13278b | 63 | |
| mdavis30 | 3:8107bb13278b | 64 | //JUST A TEST (TROY) |
| mdavis30 | 3:8107bb13278b | 65 | //position = dt; |
| mdavis30 | 3:8107bb13278b | 66 | //velocity = 0; //velocity needs to be zero, stabilize |
| mdavis30 | 3:8107bb13278b | 67 | //conv_distance = 1234; |
| mdavis30 | 3:8107bb13278b | 68 | |
| mdavis30 | 3:8107bb13278b | 69 | |
| mdavis30 | 3:8107bb13278b | 70 | x1 += x1_dot*dt; |
| mdavis30 | 3:8107bb13278b | 71 | x2 += x2_dot*dt; |
| mdavis30 | 3:8107bb13278b | 72 | |
| mdavis30 | 3:8107bb13278b | 73 | } |
| tzyoung | 0:82f27e7e99f0 | 74 | void PosVelFilter::init() |
| tzyoung | 0:82f27e7e99f0 | 75 | { |
| tzyoung | 0:82f27e7e99f0 | 76 | // run filter for 2 seconds on startup |
| tzyoung | 0:82f27e7e99f0 | 77 | float start = systemTime().read(); |
| tzyoung | 0:82f27e7e99f0 | 78 | float time = start; |
| tzyoung | 0:82f27e7e99f0 | 79 | pc().printf("\n\rWarming Up \n\r"); |
| mdavis30 | 1:ec4117689673 | 80 | |
| tzyoung | 0:82f27e7e99f0 | 81 | while ((time - start) < 1.0) { |
| tzyoung | 0:82f27e7e99f0 | 82 | update(); |
| tzyoung | 0:82f27e7e99f0 | 83 | |
| tzyoung | 0:82f27e7e99f0 | 84 | pc().printf("%5.3f \r", velocity); |
| tzyoung | 0:82f27e7e99f0 | 85 | time = systemTime().read(); |
| tzyoung | 0:82f27e7e99f0 | 86 | } |
| tzyoung | 0:82f27e7e99f0 | 87 | pc().printf("\n\r"); |
| tzyoung | 0:82f27e7e99f0 | 88 | while (1) { |
| tzyoung | 0:82f27e7e99f0 | 89 | update(); |
| tzyoung | 0:82f27e7e99f0 | 90 | |
| tzyoung | 0:82f27e7e99f0 | 91 | if (abs(velocity) > 0.005) { |
| tzyoung | 0:82f27e7e99f0 | 92 | pc().printf("Waiting to stablize. velocity: %5.3f \r", velocity); |
| tzyoung | 0:82f27e7e99f0 | 93 | |
| tzyoung | 0:82f27e7e99f0 | 94 | } else { |
| tzyoung | 0:82f27e7e99f0 | 95 | pc().printf("\n\rreading stabilized\n\r"); |
| tzyoung | 0:82f27e7e99f0 | 96 | break; |
| tzyoung | 0:82f27e7e99f0 | 97 | } |
| tzyoung | 0:82f27e7e99f0 | 98 | } |
| tzyoung | 0:82f27e7e99f0 | 99 | |
| tzyoung | 0:82f27e7e99f0 | 100 | } |
| tzyoung | 0:82f27e7e99f0 | 101 | |
| mdavis30 | 3:8107bb13278b | 102 | void PosVelFilter::init_la() |
| mdavis30 | 3:8107bb13278b | 103 | { |
| mdavis30 | 3:8107bb13278b | 104 | // run filter for 2 seconds on startup |
| mdavis30 | 3:8107bb13278b | 105 | float start = systemTime().read(); |
| mdavis30 | 3:8107bb13278b | 106 | float time = start; |
| mdavis30 | 3:8107bb13278b | 107 | pc().printf("\n\rWarming Up \n\r"); |
| mdavis30 | 3:8107bb13278b | 108 | |
| mdavis30 | 3:8107bb13278b | 109 | while ((time - start) < 1.0) { |
| mdavis30 | 3:8107bb13278b | 110 | update_la(); |
| mdavis30 | 3:8107bb13278b | 111 | |
| mdavis30 | 3:8107bb13278b | 112 | pc().printf("%5.3f \r", velocity); |
| mdavis30 | 3:8107bb13278b | 113 | time = systemTime().read(); |
| mdavis30 | 3:8107bb13278b | 114 | } |
| mdavis30 | 3:8107bb13278b | 115 | pc().printf("\n\r"); |
| mdavis30 | 3:8107bb13278b | 116 | while (1) { |
| mdavis30 | 3:8107bb13278b | 117 | update(); |
| mdavis30 | 3:8107bb13278b | 118 | |
| mdavis30 | 3:8107bb13278b | 119 | if (abs(velocity) > 0.005) { |
| mdavis30 | 3:8107bb13278b | 120 | pc().printf("Waiting to stablize. velocity: %5.3f \r", velocity); |
| mdavis30 | 3:8107bb13278b | 121 | |
| mdavis30 | 3:8107bb13278b | 122 | } else { |
| mdavis30 | 3:8107bb13278b | 123 | pc().printf("\n\rreading stabilized\n\r"); |
| mdavis30 | 3:8107bb13278b | 124 | break; |
| mdavis30 | 3:8107bb13278b | 125 | } |
| mdavis30 | 3:8107bb13278b | 126 | } |
| mdavis30 | 3:8107bb13278b | 127 | |
| mdavis30 | 3:8107bb13278b | 128 | } |
| tzyoung | 0:82f27e7e99f0 | 129 | |
| tzyoung | 0:82f27e7e99f0 | 130 | float PosVelFilter::getPosition() |
| tzyoung | 0:82f27e7e99f0 | 131 | { |
| tzyoung | 0:82f27e7e99f0 | 132 | return position; |
| tzyoung | 0:82f27e7e99f0 | 133 | } |
| tzyoung | 0:82f27e7e99f0 | 134 | |
| tzyoung | 0:82f27e7e99f0 | 135 | float PosVelFilter::getVelocity() |
| tzyoung | 0:82f27e7e99f0 | 136 | { |
| tzyoung | 0:82f27e7e99f0 | 137 | return velocity; |
| tzyoung | 0:82f27e7e99f0 | 138 | } |
| tzyoung | 0:82f27e7e99f0 | 139 | |
| tzyoung | 0:82f27e7e99f0 | 140 | float PosVelFilter::getDt() |
| tzyoung | 0:82f27e7e99f0 | 141 | { |
| tzyoung | 0:82f27e7e99f0 | 142 | return dt; |
| tzyoung | 0:82f27e7e99f0 | 143 | } |
| tzyoung | 0:82f27e7e99f0 | 144 | |
| tzyoung | 0:82f27e7e99f0 | 145 | void PosVelFilter::writeWn(float wn) |
| tzyoung | 0:82f27e7e99f0 | 146 | { |
| tzyoung | 0:82f27e7e99f0 | 147 | w_n = wn; |
| tzyoung | 0:82f27e7e99f0 | 148 | pc().printf("\n\rWn set to: %f", w_n); |
| tnhnrl | 2:992e774dc62a | 149 | } |
| tnhnrl | 2:992e774dc62a | 150 | |
| tnhnrl | 2:992e774dc62a | 151 | float PosVelFilter::get_conv_distance() |
| tnhnrl | 2:992e774dc62a | 152 | { |
| tnhnrl | 2:992e774dc62a | 153 | return conv_distance; |
| tnhnrl | 2:992e774dc62a | 154 | } |
| tnhnrl | 2:992e774dc62a | 155 | |
| tnhnrl | 2:992e774dc62a | 156 | float PosVelFilter::get_curr_time() |
| tnhnrl | 2:992e774dc62a | 157 | { |
| tnhnrl | 2:992e774dc62a | 158 | return curr_time; |
| tnhnrl | 2:992e774dc62a | 159 | } |
| tnhnrl | 2:992e774dc62a | 160 | |
| tnhnrl | 2:992e774dc62a | 161 | float PosVelFilter::get_last_time() |
| tnhnrl | 2:992e774dc62a | 162 | { |
| tnhnrl | 2:992e774dc62a | 163 | return last_time; |
| tnhnrl | 2:992e774dc62a | 164 | } |
| tnhnrl | 2:992e774dc62a | 165 | |
| tnhnrl | 2:992e774dc62a | 166 | float PosVelFilter::get_x1_dot() |
| tnhnrl | 2:992e774dc62a | 167 | { |
| tnhnrl | 2:992e774dc62a | 168 | return x1_dot; |
| tnhnrl | 2:992e774dc62a | 169 | } |
| tnhnrl | 2:992e774dc62a | 170 | |
| tnhnrl | 2:992e774dc62a | 171 | float PosVelFilter::get_x2_dot() |
| tnhnrl | 2:992e774dc62a | 172 | { |
| tnhnrl | 2:992e774dc62a | 173 | return x2_dot; |
| tnhnrl | 2:992e774dc62a | 174 | } |
| tnhnrl | 2:992e774dc62a | 175 | |
| tnhnrl | 2:992e774dc62a | 176 | float PosVelFilter::get_x1() |
| tnhnrl | 2:992e774dc62a | 177 | { |
| tnhnrl | 2:992e774dc62a | 178 | return x1; |
| tnhnrl | 2:992e774dc62a | 179 | } |
| tnhnrl | 2:992e774dc62a | 180 | |
| tnhnrl | 2:992e774dc62a | 181 | float PosVelFilter::get_x2() |
| tnhnrl | 2:992e774dc62a | 182 | { |
| tnhnrl | 2:992e774dc62a | 183 | return x2; |
| tzyoung | 0:82f27e7e99f0 | 184 | } |