Revision 44:555136de33e4, committed 2013-04-12

Comitter:

xiaxia686

Date:

Fri Apr 12 16:46:42 2013 +0000

Parent:

43:6504d85d85b4

Parent:

21:c592bf6a6a2d

Child:

45:77cf6375348a

Commit message:

colour sensors calibrated and merged with main stuff;

Changed in this revision

--- a/Sensors/CakeSensor/CakeSensor.h	Thu Apr 11 19:49:46 2013 +0000
+++ b/Sensors/CakeSensor/CakeSensor.h	Fri Apr 12 16:46:42 2013 +0000
@@ -16,6 +16,7 @@
         //float d = 5.5/(Distance()-0.13);
         float d = 7.53/(Distance()-0.022);
         d = (d < 6 || d > 30)? -1:d;
+        
         return d;
     }
 };

--- a/Sensors/Colour/Colour.cpp	Thu Apr 11 19:49:46 2013 +0000
+++ b/Sensors/Colour/Colour.cpp	Fri Apr 12 16:46:42 2013 +0000
@@ -2,29 +2,103 @@
 // Eurobot13 Colour.cpp
 
 #include "Colour.h"
+#include "mbed.h"
+#include "math.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;
+Colour::Colour(PinName blue_led,
+               PinName red_led,
+               PinName pt,
+               ArmEnum arm)
+    : _blue_led(blue_led),
+      _red_led(red_led),
+      _pt(pt),
+      _arm(arm)
+{
+
+
+    _ticker.attach(this, &Colour::_Blink, 0.01);
+    
+    if (arm == UPPER){
+    red_correction_factor = UPPERARM_CORRECTION;
     }
-    avg = x / measureNum;
-    stdev = sqrt(x2 / measureNum - avg*avg);
-    LedsOff();
-    //pc.printf("Phototransistor Analog is: %f\t%f\n\r", avg, stdev);
+    else if (arm == LOWER){
+    red_correction_factor = LOWERARM_CORRECTION;
+    }
+    else {
+    red_correction_factor = 0.00f;
+    }
+
+
 }
 
-bool Colour::isColour(DigitalOut &led, const float &avg, const float &stdev, const float numstddev){
-    float avg2, stdev2;
-    ReadLed(led, avg2, stdev2);
+void Colour::_Blink (void)
+{
+    static int toggle_colour = 0;
+    static float blue = 0;
+    static float blue_buff[BUFF_SIZE];
+    static float red = 0;
+    static float red_buff[BUFF_SIZE];
+    static float noise = 0;
+    static float noise_buff[BUFF_SIZE];
+    
+    static int buff_pointer;
+
+    if (toggle_colour == 0) {
+
+        volatile float noise_temp = _pt.read();
+        noise += (noise_temp - noise_buff[buff_pointer])/BUFF_SIZE;
+        noise_buff[buff_pointer] = noise_temp;
+        
+        buff_pointer = (buff_pointer + 1) % BUFF_SIZE;
+
+
+        _SNR = 20.0f*log10(hypot(blue,red)/noise);
+
+        volatile double blue_base = (blue - noise);
+        volatile double red_base = (red - noise)*red_correction_factor;
+        _colour = atan2(red_base,blue_base);
 
-    if (avg + numstddev*stdev < avg2 - numstddev*stdev2) {
-        return true;
+        //toggles leds for the next state
+        _blue_led = 1;
+        _red_led = 0;
+    } else if (toggle_colour == 1) {
+        volatile float blue_temp = _pt.read();
+        blue += (blue_temp - blue_buff[buff_pointer])/BUFF_SIZE;
+        blue_buff[buff_pointer] = blue_temp;        
+        //toggles leds for the next state
+        _blue_led = 0;
+        _red_led = 1;
+    } else if (toggle_colour == 2) {
+        volatile float red_temp = _pt.read();
+        red += (red_temp - red_buff[buff_pointer])/BUFF_SIZE;
+        red_buff[buff_pointer] = red_temp;          
+        //toggles leds for the next state
+        _blue_led = 0;
+        _red_led = 0;
+    }
+
+
+
+
+    toggle_colour = (toggle_colour + 1) % 3;
+
+
+}
+
+ColourEnum Colour::getColour()
+{
+    if (_SNR > SNR_THRESHOLD_DB) {
+        if ((_colour >= -30*PI/180) && (_colour < 30*PI/180)) {
+            return BLUE;
+        } else         if ((_colour >= 30*PI/180) && (_colour < 60*PI/180)) {
+            return WHITE;
+        } else         if ((_colour >= 60*PI/180) && (_colour < 120*PI/180)) {
+            return RED;
+        } else {
+            return BLACK;
+        }
     } else {
-        return false;
+        return BLACK;
     }
-}
\ No newline at end of file
+
+}

--- a/Sensors/Colour/Colour.h	Thu Apr 11 19:49:46 2013 +0000
+++ b/Sensors/Colour/Colour.h	Fri Apr 12 16:46:42 2013 +0000
@@ -1,51 +1,41 @@
 
 // Eurobot13 Colour.h
 
-//red led use 45ohm
-//blue led use 10ohm
 
 #include "mbed.h"
+#include "globals.h"
 
-enum ColourEnum {BLUE, RED, WHITE, INCONCLUSIVE, BUG};
+#define BUFF_SIZE 10
+#define SNR_THRESHOLD_DB 4
+
+#define UPPERARM_CORRECTION 2.310f
+#define LOWERARM_CORRECTION 1.000f
+
+
+enum ColourEnum {BLUE=0, RED, WHITE, BLACK};
+enum ArmEnum {UPPER=0, LOWER};
 
 class Colour{
-private:
-    DigitalOut blue;   float bavg, bstdev;
-    DigitalOut red;    float ravg, rstdev; 
-    AnalogIn pt;
+public:
+    Colour(
+    PinName blue_led, 
+    PinName red_led,
+    PinName pt,
+    ArmEnum arm);
     
-public:
-    Colour(PinName bluePin, PinName redPin, PinName phototransistorPin)
-        : blue(bluePin)
-        , red (redPin)
-        , pt (phototransistorPin)
-        {
-        LedsOff();
-    }
+    ColourEnum getColour();
 
-    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 = 0; red = 0;}
-    void ReadLed (DigitalOut &led, float &avg, float &stdev, const int measureNum = 25); // Colour.cpp
-    bool isColour(DigitalOut &led, const float &avg, const float &stdev, const float numstddev = 2); // Colour.cpp
+    Ticker _ticker;
+    DigitalOut _blue_led;
+    DigitalOut _red_led;
+    AnalogIn _pt;
+    ArmEnum _arm;
+    
+    float red_correction_factor;
+    double _colour;
+    double _SNR;
+    void _Blink (void);
     
 };
\ No newline at end of file

--- a/globals.h	Thu Apr 11 19:49:46 2013 +0000
+++ b/globals.h	Fri Apr 12 16:46:42 2013 +0000
@@ -62,7 +62,8 @@
 const PinName P_DISTANCE_SENSOR = p15;
 const PinName P_FWD_DISTANCE_SENSOR = p16;
 
-const PinName P_COLOR_SENSOR_IN = p20;
+const PinName P_COLOR_SENSOR_IN_UPPER = p20;
+const PinName P_COLOR_SENSOR_IN_LOWER = p19;
 
 const PinName P_MOT_LEFT_A     = p21;
 const PinName P_MOT_LEFT_B     = p22;
@@ -74,8 +75,10 @@
 const PinName P_ENC_LEFT_A      = p27;
 const PinName P_ENC_LEFT_B      = p28;
 
-const PinName P_COLOR_SENSOR_RED = p29;
-const PinName P_COLOR_SENSOR_BLUE = p30;
+const PinName P_COLOR_SENSOR_RED_UPPER = p29;
+const PinName P_COLOR_SENSOR_BLUE_UPPER = p30;
+const PinName P_COLOR_SENSOR_RED_LOWER = p11;
+const PinName P_COLOR_SENSOR_BLUE_LOWER = p10;
 
 
 

--- a/main.cpp	Thu Apr 11 19:49:46 2013 +0000
+++ b/main.cpp	Fri Apr 12 16:46:42 2013 +0000
@@ -22,20 +22,32 @@
 void motorsandservostest();
 void armtest();
 void motortestline();
-void ledtest();
-void phototransistortest();
-void ledphototransistortest();
 void colourtest();
+void pt_test();
 void cakesensortest();
 void printingtestthread(void const*);
 void printingtestthread2(void const*);
 void feedbacktest();
 
-int main() {
-    
-/*****************
- *   Test Code   *
- *****************/
+// bytes packing for peer to peer communication
+typedef union {
+    struct _data {
+        unsigned char header[3];
+        unsigned char ID;
+        float value;
+        float aux;
+    }  data;
+    unsigned char type_char[sizeof(_data)];
+} bytepack_t;
+
+Serial pc(USBTX, USBRX);
+
+int main()
+{
+
+    /*****************
+     *   Test Code   *
+     *****************/
     //motortest();
     //encodertest();
     //motorencodetest();
@@ -49,8 +61,8 @@
     //colourtest(); // Red SnR too low
     //cakesensortest();
     //feedbacktest();
-    
-     /*
+
+    /*
     DigitalOut l1(LED1);
     Thread p(Printing::printingloop,        NULL,   osPriorityNormal,   2048);
     l1=1;
@@ -61,12 +73,10 @@
     
     SystemTime.start();
     
-    Serial pc(USBTX, USBRX);
+    
     pc.baud(115200);
     
     InitSerial();
-    //while(1)
-    //    printbuff();
     wait(1);
     Kalman::KalmanInit();
     
@@ -77,7 +87,6 @@
     
     Ticker motorcontroltestticker;
     motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
-
     // ai layer thread
     Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
     
@@ -88,14 +97,18 @@
     Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
 
     measureCPUidle(); //repurpose thread for idle measurement
+    //colourtest();
+    //pt_test();
+    while(true) {
     Thread::wait(osWaitForever);
-
+    }
 }
 
 #include <cstdlib>
 using namespace std;
 
-void printingtestthread(void const*){
+void printingtestthread(void const*)
+{
     const char ID = 1;
     float buffer[3] = {ID};
     Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
@@ -108,7 +121,8 @@
     }
 }
 
-void printingtestthread2(void const*){
+void printingtestthread2(void const*)
+{
     const char ID = 2;
     float buffer[5] = {ID};
     Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
@@ -126,117 +140,132 @@
 void feedbacktest(){
     //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
-    
+
     Kalman::State state;
-    
+
     float Pgain = -0.01;
     float fwdspeed = -400/3.0f;
     Timer timer;
     timer.start();
-    
-    while(true){
+
+    while(true) {
         float expecdist = fwdspeed * timer.read();
         state = Kalman::getState();
         float errleft = left_encoder.getTicks() - (expecdist);
         float errright = right_encoder.getTicks() - expecdist;
-        
+
         mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
         mright(max(min(errright*Pgain, 0.4f), -0.4f));
     }
 }
 */
 
-void cakesensortest(){
+void cakesensortest()
+{
     wait(1);
     printf("cakesensortest");
-    
-    CakeSensor cs(P_COLOR_SENSOR_IN);
-    while(true){
+
+    CakeSensor cs(P_DISTANCE_SENSOR);
+    while(true) {
         wait(0.1);
         printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
-        }
+    }
 }
 
-void colourtest(){
-    Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN);
-    c.Calibrate();
-    while(true){
+void colourtest()
+{
+    //Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN,UPPER);
+    Colour c(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER);
+
+    while(true) {
         wait(0.1);
+
         ColourEnum ce = c.getColour();
-        switch(ce){
+        switch(ce) {
             case BLUE :
-                printf("BLUE\n\r");
+                printf("BLUE\n");
                 break;
             case RED:
-                printf("RED\n\r");
+                printf("RED\n");
                 break;
             case WHITE:
-                printf("WHITE\n\r");
+                printf("WHITE\n");
                 break;
-            case INCONCLUSIVE:
-                printf("INCONCLUSIVE\n\r");
+            case BLACK:
+                printf("BLACK\n");
                 break;
             default:
-                printf("BUG\n\r");
+                printf("BUG\n");
         }
+
     }
 
 }
 
-
-void ledphototransistortest(){
-    DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
-    AnalogIn pt(P_COLOR_SENSOR_IN); 
-    Serial pc(USBTX, USBRX);
+void pt_test()
+{
+    DigitalOut _blue_led(p10);
+    DigitalOut _red_led(p11);
+    AnalogIn _pt(p19);
+    
+    bytepack_t datapackage;
+    // first 3 bytes of header is used for alignment
+    datapackage.data.header[0] = 0xFF;
+    datapackage.data.header[1] = 0xFF;
+    datapackage.data.header[2] = 0xFF;
+    while(true) {
+        //toggles leds for the next state
+        _blue_led = 1;
+        _red_led = 0;
+        wait(0.01);
+        volatile float blue_temp = _pt.read();
 
-    while(true){
-        blue = 0; red = 0;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (none): %f \n\r", pt.read());
-        }
-    
-        blue = 1; red = 0;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
-        }
-        blue = 0; red = 1;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
+
+        datapackage.data.ID = (unsigned char)0;
+        datapackage.data.value = blue_temp;
+        datapackage.data.aux = 0;
+        for (int i = 0; i < sizeof(datapackage); i++) {
+            //mbed_serial.putc(datapackage.type_char[i]);
+            pc.putc(datapackage.type_char[i]);
         }
-        blue = 1; red = 1;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (both): %f \n\r", pt.read());
+
+        _blue_led = 0;
+        _red_led = 1;
+        wait(0.01);
+        volatile float red_temp = _pt.read();
+
+
+        datapackage.data.ID = (unsigned char)1;
+        datapackage.data.value = red_temp;
+        datapackage.data.aux = 0;
+        for (int i = 0; i < sizeof(datapackage); i++) {
+            //mbed_serial.putc(datapackage.type_char[i]);
+            pc.putc(datapackage.type_char[i]);
         }
-    } 
-}
 
-void phototransistortest(){
-    AnalogIn pt(P_COLOR_SENSOR_IN); 
-    while(true){
-        wait(0.1);
-        printf("Phototransistor Analog is: %f \n\r", pt.read());
-    }
+        _blue_led = 0;
+        _red_led = 0;
+        wait(0.01);
+        volatile float noise_temp = _pt.read();
 
-}
 
-void ledtest(){
-    DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
-    while(true){
-        blue = 1; red = 0;
-        wait(0.2);
-        blue = 0; red = 1;
-        wait(0.2);
-    
+        datapackage.data.ID = (unsigned char)2;
+        datapackage.data.value = noise_temp;
+        datapackage.data.aux = 0;
+        for (int i = 0; i < sizeof(datapackage); i++) {
+            //mbed_serial.putc(datapackage.type_char[i]);
+            pc.putc(datapackage.type_char[i]);
+        }
+
     }
 }
 
-void armtest(){
+
+
+void armtest()
+{
     Arm white(p26), black(p25, false, 0.0005, 180);
-    while(true){
+    while(true) {
         white(0);
         black(0);
         wait(1);
@@ -247,27 +276,29 @@
 }
 
 
-void motorsandservostest(){
+void motorsandservostest()
+{
     Encoder Eleft(p27, p28), Eright(p30, p29);
     MainMotor mleft(p24,p23), mright(p21,p22);
     Arm sTop(p25), sBottom(p26);
     //Serial pc(USBTX, USBRX);
     const float speed = 0.0;
     const float dspeed = 0.0;
-    
+
     Timer servoTimer;
-    mleft(speed); mright(speed);
+    mleft(speed);
+    mright(speed);
     servoTimer.start();
-    while (true){
+    while (true) {
         printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
-        if (Eleft.getTicks() < Eright.getTicks()){
+        if (Eleft.getTicks() < Eright.getTicks()) {
             mleft(speed);
             mright(speed - dspeed);
         } else {
             mright(speed);
             mleft(speed - dspeed);
         }
-        if (servoTimer.read() < 1){
+        if (servoTimer.read() < 1) {
             sTop.clockwise();
         } else if (servoTimer.read() < 4) {
             sTop.halt();
@@ -279,33 +310,37 @@
             //Led=0;
         } else if (servoTimer.read() < 7) {
             sBottom.halt();
-        }else {
+        } else {
             sTop.anticlockwise();
         }
         if (servoTimer.read() >= 9) servoTimer.reset();
     }
 }
 
-void motortestline(){
+void motortestline()
+{
     MainMotor mleft(p24,p23), mright(p21,p22);
     const float speed = 0.2;
-    mleft(speed); mright(speed);
+    mleft(speed);
+    mright(speed);
     while(true) wait(1);
 }
 
-void motorencodetestline(){
+void motorencodetestline()
+{
     Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
     //Serial pc(USBTX, USBRX);
     const float speed = 0.2;
     const float dspeed = 0.1;
 
-    mleft(speed); mright(speed);
-    while (true){
-    //left 27 cm = 113 -> 0.239 cm/pulse
-    //right 27 cm = 72 -> 0.375 cm/pulse
+    mleft(speed);
+    mright(speed);
+    while (true) {
+        //left 27 cm = 113 -> 0.239 cm/pulse
+        //right 27 cm = 72 -> 0.375 cm/pulse
         printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375));
-        if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375){
+        if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375) {
             mright(speed - dspeed);
         } else {
             mright(speed + dspeed);
@@ -314,45 +349,55 @@
 
 }
 
-void motorencodetest(){
+void motorencodetest()
+{
     Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
     Serial pc(USBTX, USBRX);
-    
+
     const float speed = -0.3;
     const int enc = -38;
-    while(true){
-        mleft(speed); mright(0);
-        while(Eleft.getTicks()>enc){
+    while(true) {
+        mleft(speed);
+        mright(0);
+        while(Eleft.getTicks()>enc) {
             printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
         }
-        Eleft.reset(); Eright.reset();
-        mleft(0); mright(speed);
-        while(Eright.getTicks()>enc){
+        Eleft.reset();
+        Eright.reset();
+        mleft(0);
+        mright(speed);
+        while(Eright.getTicks()>enc) {
             printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
         }
-        Eleft.reset(); Eright.reset();
+        Eleft.reset();
+        Eright.reset();
     }
 }
 
-void encodertest(){
+void encodertest()
+{
     Encoder E1(P_ENC_LEFT_A, P_ENC_LEFT_B);
     //Encoder E2(P_ENC_RIGHT_A, P_ENC_RIGHT_B);
     Serial pc(USBTX, USBRX);
-    while(true){
+    while(true) {
         wait(0.1);
         printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks());
     }
 
 }
-void motortest(){
+void motortest()
+{
     MainMotor mright(p22,p21), mleft(p23,p24);
     while(true) {
         wait(1);
-        mleft(0.8); mright(0.8);
+        mleft(0.8);
+        mright(0.8);
         wait(1);
-        mleft(-0.2); mright(0.2);
+        mleft(-0.2);
+        mright(0.2);
         wait(1);
-        mleft(0); mright(0);
+        mleft(0);
+        mright(0);
     }
 }
\ No newline at end of file