Affordable Smart Wheelchair / IMU6050Ver11

a

IMU6050/filter.cpp@17:f2e2692762ac, 2019-07-09 (annotated)

Committer:
JesiMiranda
Date:
Tue Jul 09 23:36:51 2019 +0000
Revision:
17:f2e2692762ac
Parent:
12:6efce6d008f8 
gg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jahutchi 12:6efce6d008f8 1 float lowPass(float sample)
jahutchi 12:6efce6d008f8 2 {
jahutchi 12:6efce6d008f8 3 static const float a[4] = {1.00000000e+00,-2.77555756e-16,3.33333333e-01,-1.85037171e-17};
jahutchi 12:6efce6d008f8 4 static const float b[4] = {0.16666667,0.5,0.5,0.16666667};
jahutchi 12:6efce6d008f8 5 // x array for holding recent inputs (newest input as index 0, delay of 1 at index 1, etc.
jahutchi 12:6efce6d008f8 6 static volatile float x[4] = {0};
jahutchi 12:6efce6d008f8 7 // x array for holding recent inputs (newest input as index 0, delay of 1 at index 1, etc.
jahutchi 12:6efce6d008f8 8 static volatile float y[4] = {0};
jahutchi 12:6efce6d008f8 9 x[0] = sample;
jahutchi 12:6efce6d008f8 10 // Calculate the output filtered signal based on a weighted sum of previous inputs/outputs
jahutchi 12:6efce6d008f8 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]);
jahutchi 12:6efce6d008f8 12 y[0] /= a[0];
jahutchi 12:6efce6d008f8 13 // Shift the input signals by one timestep to prepare for the next call to this function
jahutchi 12:6efce6d008f8 14 x[3] = x[2];
jahutchi 12:6efce6d008f8 15 x[2] = x[1];
jahutchi 12:6efce6d008f8 16 x[1] = x[0];
jahutchi 12:6efce6d008f8 17 // Shift the previously calculated output signals by one time step to prepare for the next call to this function
jahutchi 12:6efce6d008f8 18 y[3] = y[2];
jahutchi 12:6efce6d008f8 19 y[2] = y[1];
jahutchi 12:6efce6d008f8 20 y[1] = y[0];
jahutchi 12:6efce6d008f8 21 return y[0];
jahutchi 12:6efce6d008f8 22 }
jahutchi 12:6efce6d008f8 23
jahutchi 12:6efce6d008f8 24 float boxcar(float sample)
jahutchi 12:6efce6d008f8 25 {
jahutchi 12:6efce6d008f8 26 static const int boxcarWidth = 30; // Change this value to alter boxcar length
jahutchi 12:6efce6d008f8 27 static float recentSamples[boxcarWidth] = {0}; // hold onto recent samples
jahutchi 12:6efce6d008f8 28 static int readIndex = 0; // the index of the current reading
jahutchi 12:6efce6d008f8 29 static float total = 0; // the running total
jahutchi 12:6efce6d008f8 30 static float average = 0; // the average
jahutchi 12:6efce6d008f8 31 // subtract the last reading:
jahutchi 12:6efce6d008f8 32 total = total - recentSamples[readIndex];
jahutchi 12:6efce6d008f8 33 // add new sample to list (overwrite oldest sample)
jahutchi 12:6efce6d008f8 34 recentSamples[readIndex] = sample;
jahutchi 12:6efce6d008f8 35 // add the reading to the total:
jahutchi 12:6efce6d008f8 36 total = total + recentSamples[readIndex];
jahutchi 12:6efce6d008f8 37 // advance to the next position in the array:
jahutchi 12:6efce6d008f8 38 readIndex = readIndex + 1;
jahutchi 12:6efce6d008f8 39 // if we're at the end of the array...
jahutchi 12:6efce6d008f8 40 if (readIndex >= boxcarWidth) {
jahutchi 12:6efce6d008f8 41 // ...wrap around to the beginning:
jahutchi 12:6efce6d008f8 42 readIndex = 0;
jahutchi 12:6efce6d008f8 43 }
jahutchi 12:6efce6d008f8 44 // calculate the average:
jahutchi 12:6efce6d008f8 45 average = total / boxcarWidth;
jahutchi 12:6efce6d008f8 46 // send it to the computer as ASCII digits
jahutchi 12:6efce6d008f8 47 return average;
jahutchi 12:6efce6d008f8 48 }
jahutchi 12:6efce6d008f8 49 float complement(float x, float y , float ratio)
jahutchi 12:6efce6d008f8 50 {
jahutchi 12:6efce6d008f8 51 return (ratio*x + (1 - ratio)*y);
jahutchi 12:6efce6d008f8 52 }