Revision 3:717de74f6ebd, committed 2013-04-01

Comitter:

twighk

Date:

Mon Apr 01 15:33:48 2013 +0000

Parent:

2:45da48fab346

Child:

4:1be0f6c6ceae

Commit message:

Colour sensor, red SnR too Low

Changed in this revision

--- a/Others/EmergencyStop/EmergencyStop.h	Fri Mar 29 20:09:21 2013 +0000
+++ b/Others/EmergencyStop/EmergencyStop.h	Mon Apr 01 15:33:48 2013 +0000
@@ -7,11 +7,11 @@
 class EmergencyStop : public InterruptIn{
     private:
     public:
-    EmergencyStop   ( PinName pin
+    EmergencyStop   ( PinName interuptPin
                     , void (*risefunction)(void) = Actuator::haltandCatchFire
                     , void (*fallfunction)(void) = Actuator::haltandCatchFire
                     ) 
-                    : InterruptIn(pin)
+                    : InterruptIn(interuptPin)
                     {
     if (risefunction != NULL){
         rise(risefunction);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Colour/Colour.h	Mon Apr 01 15:33:48 2013 +0000
@@ -0,0 +1,76 @@
+
+// Eurobot13 Colour.h
+
+
+#include "mbed.h"
+#include "Led.h"
+#include "Phototransistor.h"
+
+enum ColourEnum {BLUE, RED, WHITE, INCONCLUSIVE, BUG};
+
+class Colour{
+private:
+    Led blue;   float bavg, bstdev;
+    Led red;    float ravg, rstdev; 
+    Phototransistor pt;
+    
+public:
+    Colour(PinName bluePin, PinName redPin, PinName phototransistorPin)
+        : blue(bluePin)
+        , red (redPin)
+        , pt (phototransistorPin)
+        {
+        LedsOff();
+    }
+
+    void Calibrate(){
+        ReadLed(blue, bavg, bstdev);
+        ReadLed( red, ravg, rstdev);
+    }
+        
+    ColourEnum getColour(){
+        bool blueb = isColour(blue, bavg, bstdev)
+            , redb = isColour( red, ravg, rstdev);
+                  
+        if      ( blueb &&  redb)
+                    {return WHITE;}
+        else if ( blueb && !redb)
+                    {return BLUE;}
+        else if (!blueb &&  redb)
+                    {return RED;}
+        else if (!blueb && !redb)
+                    {return INCONCLUSIVE;} 
+        return BUG;            
+    }
+
+private:
+    void LedsOff(){blue.off(); red.off();}
+    void ReadLed (Led &led, float &avg, float &stdev, const int measureNum = 25){
+        LedsOff(); led.on();
+        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 isColour(Led &led, const float &avg, const float &stdev, const float numstddev = 2 ){
+        float avg2, stdev2;
+        ReadLed(led, avg2, stdev2);
+        
+        if (avg + numstddev*stdev < avg2 - numstddev*stdev2){
+            return true;
+        } else {
+            return false;
+        }
+    }
+    
+
+
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Colour/Led.h	Mon Apr 01 15:33:48 2013 +0000
@@ -0,0 +1,23 @@
+#ifndef __led_h__
+#define __led_h__
+
+//Eurobot13 Led.h
+
+#include"mbed.h"
+
+class Led{
+    private:
+    DigitalOut led;
+    public:
+    Led(PinName digitalPin): led(digitalPin){ };
+    
+    void on(){
+        led = 1;
+    }
+    
+    void off(){
+        led = 0;
+    } 
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Colour/Phototransistor.h	Mon Apr 01 15:33:48 2013 +0000
@@ -0,0 +1,21 @@
+#ifndef __Phototransistor_h__
+#define __Phototransistor_h__
+
+
+// Eurobot13 Phototransistor.h
+
+class Phototransistor{
+    private:
+    AnalogIn ain;
+    
+    public:
+    Phototransistor(PinName analogInPin)
+        : ain (analogInPin)
+        {   }
+        
+    float read() {return ain.read();}    
+    operator float() {return read();}
+    
+};
+
+#endif

--- a/main.cpp	Fri Mar 29 20:09:21 2013 +0000
+++ b/main.cpp	Mon Apr 01 15:33:48 2013 +0000
@@ -1,12 +1,17 @@
+#pragma Otime // Compiler Optimisations
 
 // Eurobot13 main.cpp
 
 #include "mbed.h"
+Serial pc(USBTX, USBRX);
 
 #include "Actuators/MainMotors/MainMotor.h"
 #include "Sensors/Encoders/Encoder.h"
 #include "Actuators/Arms/Arm.h"
 #include "Others/EmergencyStop/EmergencyStop.h"
+#include "Sensors/Colour/Led.h"
+#include "Sensors/Colour/Colour.h"
+#include "Sensors/Colour/Phototransistor.h"
 
 
 
@@ -17,24 +22,102 @@
 void motorsandservostest();
 void armtest();
 void motortestline();
+void ledtest();
+void phototransistortest();
+void ledphototransistortest();
+void colourtest();
 
 int main() {
-/*  Setup Emergency stop for actuators,
-    Derive all actuators from the pure virtual actuator class
+/*  Setup EmergencyStop button for actuators.
+    Derive all actuators from the pure virtual actuator class!
 */  EmergencyStop EStop(p8); 
     
+/*****************
+ *   Test Code   *
+ *****************/
     //motortest();
     //encodertest();
     //motorencodetest();
     //motorencodetestline();
     //motorsandservostest();
-    armtest();
+    //armtest();
     //motortestline();
+    //ledtest();
+    //phototransistortest();
+    //ledphototransistortest();
+    colourtest(); // Red SnR too low
+}
+
+void colourtest(){
+    Colour c(p9,p10,p20);
+    c.Calibrate();
+    while(true){
+        wait(0.1);
+        ColourEnum ce = c.getColour();
+        switch(ce){
+            case BLUE :
+                pc.printf("BLUE\n\r");
+                break;
+            case RED:
+                pc.printf("RED\n\r");
+                break;
+            case WHITE:
+                pc.printf("WHITE\n\r");
+                break;
+            case INCONCLUSIVE:
+                pc.printf("INCONCLUSIVE\n\r");
+                break;
+            default:
+                pc.printf("BUG\n\r");
+        }
     }
 
+}
+
+
+void ledphototransistortest(){
+    Led blue(p9), red(p10);
+    Phototransistor pt(p20); 
+    Serial pc(USBTX, USBRX);
+
+    while(true){
+        blue.on();red.off();
+        for(int i = 0; i != 5; i++){
+            wait(0.1);
+            pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
+        }
+        blue.off();red.on();
+        for(int i = 0; i != 5; i++){
+            wait(0.1);
+            pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
+        }
+    } 
+}
+
+void phototransistortest(){
+    Phototransistor pt(p20); 
+    Serial pc(USBTX, USBRX);
+    while(true){
+        wait(0.1);
+        pc.printf("Phototransistor Analog is: %f \n\r", pt.read());
+    }
+
+}
+
+void ledtest(){
+    Led blue(p9), red(p10);
+    while(true){
+        blue.on();red.off();
+        wait(0.2);
+        blue.off();red.on();
+        wait(0.2);
+    
+    }
+}
+
 void armtest(){
-    Arm white(p26), black(p25, false,0.0005, 180);
-    while(1){
+    Arm white(p26), black(p25, false, 0.0005, 180);
+    while(true){
         white(0);
         black(0);
         wait(1);
@@ -137,7 +220,7 @@
     Encoder E1(p28, p27);
     Encoder E2(p29, p30);
     Serial pc(USBTX, USBRX);
-    while(1){
+    while(true){
         wait(0.1);
         pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), E2.getPoint());
     }
@@ -145,7 +228,7 @@
 }
 void motortest(){
     MainMotor mright(p22,p21), mleft(p23,p24);
-    while(1) {
+    while(true) {
         wait(1);
         mleft(0.8); mright(0.8);
         wait(1);