Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

Sensors/Colour/Colour.cpp

Committer:
madcowswe
Date:
2013-04-09
Revision:
20:70d651156779
Parent:
7:4340355261f9

File content as of revision 20:70d651156779:


// Eurobot13 Colour.cpp

#include "Colour.h"

void Colour::ReadLed (DigitalOut &led, float &avg, float &stdev, const int measureNum){
    LedsOff();
    led = 1;
    double x = 0, x2 = 0;
    for (int i = measureNum; i != 0; i--) {
        float v = pt.read();
        x += v;
        x2+= v*v;
    }
    avg = x / measureNum;
    stdev = sqrt(x2 / measureNum - avg*avg);
    LedsOff();
    //pc.printf("Phototransistor Analog is: %f\t%f\n\r", avg, stdev);
}

bool Colour::isColour(DigitalOut &led, const float &avg, const float &stdev, const float numstddev){
    float avg2, stdev2;
    ReadLed(led, avg2, stdev2);

    if (avg + numstddev*stdev < avg2 - numstddev*stdev2) {
        return true;
    } else {
        return false;
    }
}