Josh Goldberg / Mbed 2 deprecated count_pills

Dependencies:   Servo mbed

util.cpp

Committer:
jgoldberg
Date:
2014-12-11
Revision:
0:15d3cdafd311
Child:
1:9ebe670d8a19

File content as of revision 0:15d3cdafd311:

#include "mbed.h"
#include "util.h"

uint16_t mvgAvg(uint16_t val, int reset) {
    const uint32_t len = 25;
    static uint16_t vals[len] = {0};
    static uint32_t ind = 0;
    static uint32_t full = 0;
    static uint32_t sum = 0;
    
    if (reset) {
        ind = full = sum = 0;
        return 0;
    }
    if (full) {
        sum -= vals[ind];
    }
    sum += val;
    vals[ind] = val;
    
    ind++;
    if (ind >= len) {
        ind = 0;
        full = 1;
    }
    if (full) {
        return (uint16_t) (sum / len);
    }
    return (uint16_t) (sum / ind);
}

int round(float val) {
    return val > 0 ? ((int) (val + 0.5)) : ((int) (val - 0.5));
}