With pid left, right, and foward
Dependents: wheelchaircontrol wheelchaircontrolRosCom wheelchaircontrol wheelchaircontrol2 ... more
Fork of chair_BNO055 by
filter.cpp@6:ce8aa8208590, 2019-06-27 (annotated)
- Committer:
- jvfausto
- Date:
- Thu Jun 27 16:32:06 2019 +0000
- Revision:
- 6:ce8aa8208590
- Parent:
- 1:3258d62af038
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryanlin97 | 1:3258d62af038 | 1 | float lowPass(float sample) |
ryanlin97 | 1:3258d62af038 | 2 | { |
ryanlin97 | 1:3258d62af038 | 3 | static const float a[4] = {1.00000000e+00,-2.77555756e-16,3.33333333e-01,-1.85037171e-17}; |
ryanlin97 | 1:3258d62af038 | 4 | static const float b[4] = {0.16666667,0.5,0.5,0.16666667}; |
ryanlin97 | 1:3258d62af038 | 5 | // x array for holding recent inputs (newest input as index 0, delay of 1 at index 1, etc. |
ryanlin97 | 1:3258d62af038 | 6 | static volatile float x[4] = {0}; |
ryanlin97 | 1:3258d62af038 | 7 | // x array for holding recent inputs (newest input as index 0, delay of 1 at index 1, etc. |
ryanlin97 | 1:3258d62af038 | 8 | static volatile float y[4] = {0}; |
ryanlin97 | 1:3258d62af038 | 9 | x[0] = sample; |
ryanlin97 | 1:3258d62af038 | 10 | // Calculate the output filtered signal based on a weighted sum of previous inputs/outputs |
ryanlin97 | 1:3258d62af038 | 11 | y[0] = (b[0]*x[0]+b[1]*x[1]+b[2]*x[2]+b[3]*x[3])-(a[1]*y[1]+a[2]*y[2]+a[3]*y[3]); |
ryanlin97 | 1:3258d62af038 | 12 | y[0] /= a[0]; |
ryanlin97 | 1:3258d62af038 | 13 | // Shift the input signals by one timestep to prepare for the next call to this function |
ryanlin97 | 1:3258d62af038 | 14 | x[3] = x[2]; |
ryanlin97 | 1:3258d62af038 | 15 | x[2] = x[1]; |
ryanlin97 | 1:3258d62af038 | 16 | x[1] = x[0]; |
ryanlin97 | 1:3258d62af038 | 17 | // Shift the previously calculated output signals by one time step to prepare for the next call to this function |
ryanlin97 | 1:3258d62af038 | 18 | y[3] = y[2]; |
ryanlin97 | 1:3258d62af038 | 19 | y[2] = y[1]; |
ryanlin97 | 1:3258d62af038 | 20 | y[1] = y[0]; |
ryanlin97 | 1:3258d62af038 | 21 | return y[0]; |
ryanlin97 | 1:3258d62af038 | 22 | } |
ryanlin97 | 1:3258d62af038 | 23 | |
ryanlin97 | 1:3258d62af038 | 24 | float boxcar(float sample) |
ryanlin97 | 1:3258d62af038 | 25 | { |
ryanlin97 | 1:3258d62af038 | 26 | static const int boxcarWidth = 30; // Change this value to alter boxcar length |
ryanlin97 | 1:3258d62af038 | 27 | static float recentSamples[boxcarWidth] = {0}; // hold onto recent samples |
ryanlin97 | 1:3258d62af038 | 28 | static int readIndex = 0; // the index of the current reading |
ryanlin97 | 1:3258d62af038 | 29 | static float total = 0; // the running total |
ryanlin97 | 1:3258d62af038 | 30 | static float average = 0; // the average |
ryanlin97 | 1:3258d62af038 | 31 | // subtract the last reading: |
ryanlin97 | 1:3258d62af038 | 32 | total = total - recentSamples[readIndex]; |
ryanlin97 | 1:3258d62af038 | 33 | // add new sample to list (overwrite oldest sample) |
ryanlin97 | 1:3258d62af038 | 34 | recentSamples[readIndex] = sample; |
ryanlin97 | 1:3258d62af038 | 35 | // add the reading to the total: |
ryanlin97 | 1:3258d62af038 | 36 | total = total + recentSamples[readIndex]; |
ryanlin97 | 1:3258d62af038 | 37 | // advance to the next position in the array: |
ryanlin97 | 1:3258d62af038 | 38 | readIndex = readIndex + 1; |
ryanlin97 | 1:3258d62af038 | 39 | // if we're at the end of the array... |
ryanlin97 | 1:3258d62af038 | 40 | if (readIndex >= boxcarWidth) { |
ryanlin97 | 1:3258d62af038 | 41 | // ...wrap around to the beginning: |
ryanlin97 | 1:3258d62af038 | 42 | readIndex = 0; |
ryanlin97 | 1:3258d62af038 | 43 | } |
ryanlin97 | 1:3258d62af038 | 44 | // calculate the average: |
ryanlin97 | 1:3258d62af038 | 45 | average = total / boxcarWidth; |
ryanlin97 | 1:3258d62af038 | 46 | // send it to the computer as ASCII digits |
ryanlin97 | 1:3258d62af038 | 47 | return average; |
ryanlin97 | 1:3258d62af038 | 48 | } |
ryanlin97 | 1:3258d62af038 | 49 | float complement(float x, float y , float ratio) |
ryanlin97 | 1:3258d62af038 | 50 | { |
ryanlin97 | 1:3258d62af038 | 51 | return (ratio*x + (1 - ratio)*y); |
ryanlin97 | 1:3258d62af038 | 52 | } |