Question

I wrote this simple code which reads a length from the Sharp infrared sensor, end presents the average meter in cm (unit) by serial.

When write this code for the Arduino Mega board, the Arduino starts a blinking LED (pin 13) and the program does nothing. Where is the bug in this code?

#include <QueueList.h>

const int ANALOG_SHARP = 0; //Set pin data from sharp.
QueueList <float> queuea;
float cm;
float qu1;
float qu2;
float qu3;
float qu4;
float qu5;

void setup() {
    Serial.begin(9600);
}

void loop() {
    cm = read_gp2d12_range(ANALOG_SHARP); //Convert to cm (unit).
    queuea.push(cm); //Add item to queue, when I add only this line Arduino crash.
    if ( 5 <= queuea.peek()) {
        Serial.println(average());
    }
}

float read_gp2d12_range(byte pin) { //Function converting to cm (unit).
    int tmp;

    tmp = analogRead(pin);
    if (tmp < 3)
        return -1; // Invalid value.

    return (6787.0 /((float)tmp - 3.0)) - 4.0;
}

float average() { //Calculate average length
    qu1 += queuea.pop();
    qu2 += queuea.pop();
    qu3 += queuea.pop();
    qu4 += queuea.pop();
    qu5 += queuea.pop();

    float aver = ((qu1+qu2+qu3+qu4+qu5)/5);
    return aver;
}

Answer 1

I agree with the peek() -> count() error listed by vhallac. But I’ll also point out that you should consider averaging by powers of 2 unless there is a strong case to do otherwise.

The reason is that on microcontrollers, division is slow. By averaging over a power of 2 (2,4,8,16,etc.) you can simply calculate the sum and then bitshift it.

To calculate the average of 2: (v1 + v2) >> 1

To calculate the average of 4: (v1 + v2 + v3 + v4) >> 2

To calculate the average of n values (where n is a power of 2) just right bitshift the sum right by [log2(n)].

As long as the datatype for your sum variable is big enough and won’t overflow, this is much easier and much faster.

Note: this won’t work for floats in general. In fact, microcontrollers aren’t optimized for floats. You should consider converting from int (what I’m assuming you’re ADC is reading) to float at the end after the averaging rather than before.

By converting from int to float and then averaging floats you are losing more precision than averaging ints than converting the int to a float.

Other:

You’re using the += operator without initializing the variables (qu1, qu2, etc.) — it’s good practice to initialize them if you’re going to use += but it looks as if = would work fine.

For floats, I’d have written the average function as:

float average(QueueList<float> & q, int n)
{
    float sum = 0;
    for(int i=0; i<n; i++)
    {
        sum += q.pop();
    }

    return (sum / (float) n);
}

And called it: average(queuea, 5);

You could use this to average any number of sensor readings and later use the same code to later average floats in a completely different QueueList. Passing the number of readings to average as a parameter will really come in handy in the case that you need to tweak it.

TL;DR:

Here’s how I would have done it:

#include <QueueList.h>

const int ANALOG_SHARP=0;   // set pin data from sharp
const int AvgPower = 2;     // 1 for 2 readings, 2 for 4 readings, 3 for 8, etc.
const int AvgCount = pow(2,AvgPow);

QueueList <int> SensorReadings;


void setup(){
    Serial.begin(9600);
}

void loop()
{
    int reading = analogRead(ANALOG_SHARP);
    SensorReadings.push(reading);

    if(SensorReadings.count() > AvgCount)
    {
        int avg = average2(SensorReadings, AvgPower);
        Serial.println(gpd12_to_cm(avg));
    }
}

float gp2d12_to_cm(int reading)
{
    if(reading <= 3){ return -1; }

    return((6787.0 /((float)reading - 3.0)) - 4.0);
}

int average2(QueueList<int> & q, int AvgPower)
{
    int AvgCount = pow(2, AvgPower);
    long sum = 0;
    for(int i=0; i<AvgCount; i++)
    {
        sum += q.pop();
    }

    return (sum >> AvgPower);
}