Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Filter/CircularBuffer.cpp@145:37ffa3ba3862, 2017-05-03 (annotated)

Committer:
bwang
Date:
Wed May 03 12:28:05 2017 +0000
Revision:
145:37ffa3ba3862
Child:
147:c1b2379b8874
05/03/2017 08:27 - CircularBuffer moved to its own .cpp file

Who changed what in which revision?

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