Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

Sensors/Colour/Colour.h@3:717de74f6ebd, 2013-04-01 (annotated)

Committer:
twighk
Date:
Mon Apr 01 15:33:48 2013 +0000
Revision:
3:717de74f6ebd
Child:
4:1be0f6c6ceae
Colour sensor, red SnR too Low

Who changed what in which revision?

UserRevisionLine numberNew contents of line
twighk 3:717de74f6ebd 1
twighk 3:717de74f6ebd 2 // Eurobot13 Colour.h
twighk 3:717de74f6ebd 3
twighk 3:717de74f6ebd 4
twighk 3:717de74f6ebd 5 #include "mbed.h"
twighk 3:717de74f6ebd 6 #include "Led.h"
twighk 3:717de74f6ebd 7 #include "Phototransistor.h"
twighk 3:717de74f6ebd 8
twighk 3:717de74f6ebd 9 enum ColourEnum {BLUE, RED, WHITE, INCONCLUSIVE, BUG};
twighk 3:717de74f6ebd 10
twighk 3:717de74f6ebd 11 class Colour{
twighk 3:717de74f6ebd 12 private:
twighk 3:717de74f6ebd 13 Led blue; float bavg, bstdev;
twighk 3:717de74f6ebd 14 Led red; float ravg, rstdev;
twighk 3:717de74f6ebd 15 Phototransistor pt;
twighk 3:717de74f6ebd 16
twighk 3:717de74f6ebd 17 public:
twighk 3:717de74f6ebd 18 Colour(PinName bluePin, PinName redPin, PinName phototransistorPin)
twighk 3:717de74f6ebd 19 : blue(bluePin)
twighk 3:717de74f6ebd 20 , red (redPin)
twighk 3:717de74f6ebd 21 , pt (phototransistorPin)
twighk 3:717de74f6ebd 22 {
twighk 3:717de74f6ebd 23 LedsOff();
twighk 3:717de74f6ebd 24 }
twighk 3:717de74f6ebd 25
twighk 3:717de74f6ebd 26 void Calibrate(){
twighk 3:717de74f6ebd 27 ReadLed(blue, bavg, bstdev);
twighk 3:717de74f6ebd 28 ReadLed( red, ravg, rstdev);
twighk 3:717de74f6ebd 29 }
twighk 3:717de74f6ebd 30
twighk 3:717de74f6ebd 31 ColourEnum getColour(){
twighk 3:717de74f6ebd 32 bool blueb = isColour(blue, bavg, bstdev)
twighk 3:717de74f6ebd 33 , redb = isColour( red, ravg, rstdev);
twighk 3:717de74f6ebd 34
twighk 3:717de74f6ebd 35 if ( blueb && redb)
twighk 3:717de74f6ebd 36 {return WHITE;}
twighk 3:717de74f6ebd 37 else if ( blueb && !redb)
twighk 3:717de74f6ebd 38 {return BLUE;}
twighk 3:717de74f6ebd 39 else if (!blueb && redb)
twighk 3:717de74f6ebd 40 {return RED;}
twighk 3:717de74f6ebd 41 else if (!blueb && !redb)
twighk 3:717de74f6ebd 42 {return INCONCLUSIVE;}
twighk 3:717de74f6ebd 43 return BUG;
twighk 3:717de74f6ebd 44 }
twighk 3:717de74f6ebd 45
twighk 3:717de74f6ebd 46 private:
twighk 3:717de74f6ebd 47 void LedsOff(){blue.off(); red.off();}
twighk 3:717de74f6ebd 48 void ReadLed (Led &led, float &avg, float &stdev, const int measureNum = 25){
twighk 3:717de74f6ebd 49 LedsOff(); led.on();
twighk 3:717de74f6ebd 50 double x = 0, x2 = 0;
twighk 3:717de74f6ebd 51 for (int i = measureNum; i != 0; i--){
twighk 3:717de74f6ebd 52 float v = pt.read();
twighk 3:717de74f6ebd 53 x += v;
twighk 3:717de74f6ebd 54 x2+= v*v;
twighk 3:717de74f6ebd 55 }
twighk 3:717de74f6ebd 56 avg = x / measureNum;
twighk 3:717de74f6ebd 57 stdev = sqrt(x2 / measureNum - avg*avg);
twighk 3:717de74f6ebd 58 LedsOff();
twighk 3:717de74f6ebd 59
twighk 3:717de74f6ebd 60 pc.printf("Phototransistor Analog is: %f\t%f\n\r", avg, stdev);
twighk 3:717de74f6ebd 61 }
twighk 3:717de74f6ebd 62
twighk 3:717de74f6ebd 63 bool isColour(Led &led, const float &avg, const float &stdev, const float numstddev = 2 ){
twighk 3:717de74f6ebd 64 float avg2, stdev2;
twighk 3:717de74f6ebd 65 ReadLed(led, avg2, stdev2);
twighk 3:717de74f6ebd 66
twighk 3:717de74f6ebd 67 if (avg + numstddev*stdev < avg2 - numstddev*stdev2){
twighk 3:717de74f6ebd 68 return true;
twighk 3:717de74f6ebd 69 } else {
twighk 3:717de74f6ebd 70 return false;
twighk 3:717de74f6ebd 71 }
twighk 3:717de74f6ebd 72 }
twighk 3:717de74f6ebd 73
twighk 3:717de74f6ebd 74
twighk 3:717de74f6ebd 75
twighk 3:717de74f6ebd 76 };