Affordable Smart Wheelchair / Mbed OS Version1-2

Fully integrated ToF/IMU codes

Dependencies:   QEI2 PID Watchdog VL53L1X_Filter ros_lib_kinetic

wheelchairControlSideTof/IMU6050/filter.cpp@21:d1faccb96146, 2019-07-09 (annotated)

Committer:
isagmz
Date:
Tue Jul 09 17:52:32 2019 +0000
Revision:
21:d1faccb96146
Finished IMU side sensor code

Who changed what in which revision?

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