Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Filter/Filter.cpp@144:a9e7fa1c98d7, 2017-05-03 (annotated)

Committer:
bwang
Date:
Wed May 03 12:24:28 2017 +0000
Revision:
144:a9e7fa1c98d7
Child:
145:37ffa3ba3862
05/03/2017 08:24 - added circular buffer code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 144:a9e7fa1c98d7 1 #include "mbed.h"
bwang 144:a9e7fa1c98d7 2 #include "math.h"
bwang 144:a9e7fa1c98d7 3 #include "Filter.h"
bwang 144:a9e7fa1c98d7 4
bwang 144:a9e7fa1c98d7 5 using namespace std;
bwang 144:a9e7fa1c98d7 6
bwang 144:a9e7fa1c98d7 7 CircularBuffer::CircularBuffer(int length) {
bwang 144:a9e7fa1c98d7 8 _length = length;
bwang 144:a9e7fa1c98d7 9 oldest_index = 0;
bwang 144:a9e7fa1c98d7 10 newest_index = -1;
bwang 144:a9e7fa1c98d7 11 num = 0;
bwang 144:a9e7fa1c98d7 12 sum = 0.0f;
bwang 144:a9e7fa1c98d7 13
bwang 144:a9e7fa1c98d7 14 buf = (float*)malloc(_length * sizeof(float));
bwang 144:a9e7fa1c98d7 15 sorted = (float*)malloc(_length * sizeof(float));
bwang 144:a9e7fa1c98d7 16 for (int i = 0; i < _length; i++) {
bwang 144:a9e7fa1c98d7 17 buf[i] = 0.0f;
bwang 144:a9e7fa1c98d7 18 sorted[i] = 0.0f;
bwang 144:a9e7fa1c98d7 19 }
bwang 144:a9e7fa1c98d7 20 }
bwang 144:a9e7fa1c98d7 21
bwang 144:a9e7fa1c98d7 22 float &CircularBuffer::at(int index) {
bwang 144:a9e7fa1c98d7 23 int actual = oldest_index + index;
bwang 144:a9e7fa1c98d7 24 if (actual >= _length) actual -= _length;
bwang 144:a9e7fa1c98d7 25 return buf[actual];
bwang 144:a9e7fa1c98d7 26 }
bwang 144:a9e7fa1c98d7 27
bwang 144:a9e7fa1c98d7 28 void CircularBuffer::add(float x) {
bwang 144:a9e7fa1c98d7 29 if (num < _length) {
bwang 144:a9e7fa1c98d7 30 newest_index++;
bwang 144:a9e7fa1c98d7 31 buf[newest_index] = x;
bwang 144:a9e7fa1c98d7 32 sum += x;
bwang 144:a9e7fa1c98d7 33
bwang 144:a9e7fa1c98d7 34 /*insert x into sorted array*/
bwang 144:a9e7fa1c98d7 35 int i = num;
bwang 144:a9e7fa1c98d7 36 while (i > 0 && sorted[i] > x) {
bwang 144:a9e7fa1c98d7 37 sorted[i] = sorted[i - 1];
bwang 144:a9e7fa1c98d7 38 i--;
bwang 144:a9e7fa1c98d7 39 }
bwang 144:a9e7fa1c98d7 40 sorted[i] = x;
bwang 144:a9e7fa1c98d7 41
bwang 144:a9e7fa1c98d7 42 num++;
bwang 144:a9e7fa1c98d7 43 }
bwang 144:a9e7fa1c98d7 44 else {
bwang 144:a9e7fa1c98d7 45 /*find sorted index of oldest element*/
bwang 144:a9e7fa1c98d7 46 int removed;
bwang 144:a9e7fa1c98d7 47 for (removed = 0; removed < _length; removed++) {
bwang 144:a9e7fa1c98d7 48 if (sorted[removed] == buf[oldest_index]) break;
bwang 144:a9e7fa1c98d7 49 }
bwang 144:a9e7fa1c98d7 50
bwang 144:a9e7fa1c98d7 51 /*update circular buffer*/
bwang 144:a9e7fa1c98d7 52 sum -= buf[oldest_index];
bwang 144:a9e7fa1c98d7 53 oldest_index++;
bwang 144:a9e7fa1c98d7 54 if (oldest_index >= _length) oldest_index -= _length;
bwang 144:a9e7fa1c98d7 55 newest_index++;
bwang 144:a9e7fa1c98d7 56 if (newest_index >= _length) newest_index -= _length;
bwang 144:a9e7fa1c98d7 57 buf[newest_index] = x;
bwang 144:a9e7fa1c98d7 58 sum += x;
bwang 144:a9e7fa1c98d7 59
bwang 144:a9e7fa1c98d7 60 /*insert x*/
bwang 144:a9e7fa1c98d7 61 int i;
bwang 144:a9e7fa1c98d7 62 if (removed == _length - 1) {
bwang 144:a9e7fa1c98d7 63 i = _length - 1;
bwang 144:a9e7fa1c98d7 64 while (i > 0 && sorted[i] > x) {
bwang 144:a9e7fa1c98d7 65 sorted[i] = sorted[i - 1];
bwang 144:a9e7fa1c98d7 66 i--;
bwang 144:a9e7fa1c98d7 67 }
bwang 144:a9e7fa1c98d7 68 sorted[i] = x;
bwang 144:a9e7fa1c98d7 69 }
bwang 144:a9e7fa1c98d7 70 else if (removed == 0) {
bwang 144:a9e7fa1c98d7 71 i = 0;
bwang 144:a9e7fa1c98d7 72 while (i < _length - 1 && sorted[i] < x) {
bwang 144:a9e7fa1c98d7 73 sorted[i] = sorted[i + 1];
bwang 144:a9e7fa1c98d7 74 i++;
bwang 144:a9e7fa1c98d7 75 }
bwang 144:a9e7fa1c98d7 76 sorted[i] = x;
bwang 144:a9e7fa1c98d7 77 }
bwang 144:a9e7fa1c98d7 78 else if (sorted[removed - 1] <= x && sorted[removed + 1] >= x) {
bwang 144:a9e7fa1c98d7 79 sorted[removed] = x;
bwang 144:a9e7fa1c98d7 80 }
bwang 144:a9e7fa1c98d7 81 else if (sorted[removed - 1] > x) {
bwang 144:a9e7fa1c98d7 82 i = removed;
bwang 144:a9e7fa1c98d7 83 while (i > 0 && sorted[i] > x) {
bwang 144:a9e7fa1c98d7 84 sorted[i] = sorted[i - 1];
bwang 144:a9e7fa1c98d7 85 i--;
bwang 144:a9e7fa1c98d7 86 }
bwang 144:a9e7fa1c98d7 87 sorted[i] = x;
bwang 144:a9e7fa1c98d7 88 }
bwang 144:a9e7fa1c98d7 89 else {
bwang 144:a9e7fa1c98d7 90 i = removed;
bwang 144:a9e7fa1c98d7 91 while (i < _length - 1 && sorted[i] < x) {
bwang 144:a9e7fa1c98d7 92 sorted[i] = sorted[i + 1];
bwang 144:a9e7fa1c98d7 93 i++;
bwang 144:a9e7fa1c98d7 94 }
bwang 144:a9e7fa1c98d7 95 sorted[i] = x;
bwang 144:a9e7fa1c98d7 96 }
bwang 144:a9e7fa1c98d7 97 }
bwang 144:a9e7fa1c98d7 98 }
bwang 144:a9e7fa1c98d7 99
bwang 144:a9e7fa1c98d7 100 float CircularBuffer::mean() {
bwang 144:a9e7fa1c98d7 101 return sum / num;
bwang 144:a9e7fa1c98d7 102 }
bwang 144:a9e7fa1c98d7 103
bwang 144:a9e7fa1c98d7 104 float CircularBuffer::median() {
bwang 144:a9e7fa1c98d7 105 if (num < _length) {
bwang 144:a9e7fa1c98d7 106 if (num % 2 == 1) {
bwang 144:a9e7fa1c98d7 107 return sorted[(num - 1) / 2];
bwang 144:a9e7fa1c98d7 108 }
bwang 144:a9e7fa1c98d7 109 else {
bwang 144:a9e7fa1c98d7 110 return (sorted[num / 2] + sorted[num / 2 - 1]) / 2.0f;
bwang 144:a9e7fa1c98d7 111 }
bwang 144:a9e7fa1c98d7 112 }
bwang 144:a9e7fa1c98d7 113 else {
bwang 144:a9e7fa1c98d7 114 if (_length % 2 == 1) {
bwang 144:a9e7fa1c98d7 115 return sorted[(_length - 1) / 2];
bwang 144:a9e7fa1c98d7 116 }
bwang 144:a9e7fa1c98d7 117 else {
bwang 144:a9e7fa1c98d7 118 return (sorted[_length / 2] + sorted[_length / 2 - 1]) / 2.0f;
bwang 144:a9e7fa1c98d7 119 }
bwang 144:a9e7fa1c98d7 120 }
bwang 144:a9e7fa1c98d7 121 }