RoboBuggy

#include <Arduino.h>

#include "receiver.h"

#include "filter.h"

#define SIZE 5 // the array size

static char started = 0;

static int last_val_sent;

static int values[5];

static int pos;

int filter(int val){

     int i;

    // First initialization: on assumption val is never -1

    if(!started){

        for(i = 0; i < SIZE; i++){

            values[i] = val;

        }

        pos = 0; // initialize the positions of the next number to be changed

        started = 1;

        return val;

    }

    // NOT first initialization. 

    values[pos] = val;  // replace the number in the array

    pos = (pos+1)%SIZE; // increment the pos number

    // calculate average of array

    int sum = 0;

    for(i = 0; i < SIZE; i++){

        sum += values[i];

    }

    int avg = sum/SIZE;

    // return.

    return avg;

}