Revision 45:77cf6375348a, committed 2013-04-12

Comitter:

xiaxia686

Date:

Fri Apr 12 20:40:52 2013 +0000

Parent:

44:555136de33e4

Child:

46:adcd57a5e402

Commit message:

colour sensors fixed

Changed in this revision

--- a/Sensors/Colour/Colour.cpp	Fri Apr 12 16:46:42 2013 +0000
+++ b/Sensors/Colour/Colour.cpp	Fri Apr 12 20:40:52 2013 +0000
@@ -3,96 +3,102 @@
 
 #include "Colour.h"
 #include "mbed.h"
-#include "math.h"
+
 
-Colour::Colour(PinName blue_led,
-               PinName red_led,
-               PinName pt,
-               ArmEnum arm)
-    : _blue_led(blue_led),
-      _red_led(red_led),
-      _pt(pt),
-      _arm(arm)
+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;
-    }
-    else if (arm == LOWER){
-    red_correction_factor = LOWERARM_CORRECTION;
-    }
-    else {
-    red_correction_factor = 0.00f;
+
+
+
+
+    if (arm == UPPER) {
+        red_correction_factor = UPPERARM_CORRECTION;
+    } else if (arm == LOWER) {
+        red_correction_factor = LOWERARM_CORRECTION;
+    } else {
+        red_correction_factor = 0.00f;
     }
 
+    togglecolour = 0;
+    blue = 0;
+    red = 0;
+    noise = 0;
+    buff_pointer = 0;
+
+
+    for (int i = 0; i < BUFF_SIZE; i++) {
+        blue_buff[i] = 0;
+        red_buff[i] = 0;
+        noise_buff[i] = 0;
+    }
+
+    ticker.attach(this, &Colour::Blink, 0.01);
 
 }
 
-void Colour::_Blink (void)
+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) {
+    if (togglecolour == 0) {
 
-        volatile float noise_temp = _pt.read();
+        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);
+        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);
+        float blue_base = (blue - noise);
+        float red_base = (red - noise)*red_correction_factor;
+        colour = atan2(red_base,blue_base);
 
         //toggles leds for the next state
-        _blue_led = 1;
-        _red_led = 0;
-    } else if (toggle_colour == 1) {
-        volatile float blue_temp = _pt.read();
+        blue_led = 1;
+        red_led = 0;
+    } else if (togglecolour == 1) {
+        float blue_temp = pt.read();
         blue += (blue_temp - blue_buff[buff_pointer])/BUFF_SIZE;
-        blue_buff[buff_pointer] = blue_temp;        
+        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();
+        blue_led = 0;
+        red_led = 1;
+    } else if (togglecolour == 2) {
+        float red_temp = pt.read();
         red += (red_temp - red_buff[buff_pointer])/BUFF_SIZE;
-        red_buff[buff_pointer] = red_temp;          
+        red_buff[buff_pointer] = red_temp;
         //toggles leds for the next state
-        _blue_led = 0;
-        _red_led = 0;
+        blue_led = 0;
+        red_led = 0;
     }
 
 
 
 
-    toggle_colour = (toggle_colour + 1) % 3;
+    togglecolour = (togglecolour + 1) % 3;
 
 
 }
 
 ColourEnum Colour::getColour()
 {
-    if (_SNR > SNR_THRESHOLD_DB) {
-        if ((_colour >= -30*PI/180) && (_colour < 30*PI/180)) {
+    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)) {
+        } else         if ((colour >= 30*PI/180) && (colour < 60*PI/180)) {
             return WHITE;
-        } else         if ((_colour >= 60*PI/180) && (_colour < 120*PI/180)) {
+        } else         if ((colour >= 60*PI/180) && (colour < 120*PI/180)) {
             return RED;
         } else {
             return BLACK;

--- a/Sensors/Colour/Colour.h	Fri Apr 12 16:46:42 2013 +0000
+++ b/Sensors/Colour/Colour.h	Fri Apr 12 20:40:52 2013 +0000
@@ -1,9 +1,11 @@
 
 // Eurobot13 Colour.h
-
+#ifndef COLOUR_H
+#define COLOUR_H
 
 #include "mbed.h"
 #include "globals.h"
+#include "math.h"
 
 #define BUFF_SIZE 10
 #define SNR_THRESHOLD_DB 4
@@ -17,25 +19,38 @@
 
 class Colour{
 public:
+
     Colour(
     PinName blue_led, 
     PinName red_led,
     PinName pt,
     ArmEnum arm);
     
-    ColourEnum getColour();
+    virtual ColourEnum getColour();
 
 
 private:
-    Ticker _ticker;
-    DigitalOut _blue_led;
-    DigitalOut _red_led;
-    AnalogIn _pt;
-    ArmEnum _arm;
+    Ticker ticker;
+    DigitalOut blue_led;
+    DigitalOut red_led;
+    AnalogIn pt;
+    ArmEnum arm;
     
     float red_correction_factor;
-    double _colour;
-    double _SNR;
-    void _Blink (void);
+    float colour;
+    float SNR;
+    void Blink();
     
-};
\ No newline at end of file
+    int togglecolour;
+    float blue;
+    float blue_buff[BUFF_SIZE];
+    float red;
+    float red_buff[BUFF_SIZE];
+    float noise;
+    float noise_buff[BUFF_SIZE];
+    
+    int buff_pointer;    
+    
+};
+
+#endif
\ No newline at end of file

--- a/main.cpp	Fri Apr 12 16:46:42 2013 +0000
+++ b/main.cpp	Fri Apr 12 20:40:52 2013 +0000
@@ -76,28 +76,28 @@
     
     pc.baud(115200);
     
-    InitSerial();
-    wait(1);
-    Kalman::KalmanInit();
+ //   InitSerial();
+ //   wait(1);
+ //   Kalman::KalmanInit();
     
-    Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
-    Kalman::start_predict_ticker(&predictthread);
+ //   Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
+ //   Kalman::start_predict_ticker(&predictthread);
     
-    Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084);
+ //   Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084);
     
-    Ticker motorcontroltestticker;
-    motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
+ //   Ticker motorcontroltestticker;
+ //   motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
     // ai layer thread
-    Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
+ //   Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
     
     // motion layer periodic callback
-    RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
-    motion_timer.start(50);
+ //   RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
+ //   motion_timer.start(50);
 
-    Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
+ //   Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
 
-    measureCPUidle(); //repurpose thread for idle measurement
-    //colourtest();
+ //   measureCPUidle(); //repurpose thread for idle measurement
+    colourtest();
     //pt_test();
     while(true) {
     Thread::wait(osWaitForever);
@@ -174,14 +174,13 @@
 
 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);
+    Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER);
+    Colour c_lower(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(c_lower.getColour()) {
             case BLUE :
                 printf("BLUE\n");
                 break;
@@ -206,7 +205,7 @@
 {
     DigitalOut _blue_led(p10);
     DigitalOut _red_led(p11);
-    AnalogIn _pt(p19);
+    AnalogIn _pt(p18);
     
     bytepack_t datapackage;
     // first 3 bytes of header is used for alignment