Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Filter/fCircularBuffer.cpp@252:38644631ed97, 2018-11-13 (annotated)

Committer:
bwang
Date:
Tue Nov 13 17:46:23 2018 +0000
Revision:
252:38644631ed97
Parent:
224:90172915d0fb 
11/13/2018 12:45 - hitting <return> on empty line prints "\r>", so that hitting enter after intially connecting to the controller generates a prompt

Who changed what in which revision?

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