ColorSensor.cpp

00001 #include "ColorSensor.h"
00002 // カラーセンサーテストプログラム
00003 // 赤、緑、青を判定する。
00004 #define coef 1.05
00005 
00006 ColorSensor::ColorSensor(
00007     PinName dout, PinName range, 
00008     PinName ck, PinName gate, int tm)
00009 {
00010   Dout  = new DigitalIn(dout);
00011   Range = new DigitalOut(range);
00012   CK    = new DigitalOut(ck);
00013   Gate  = new DigitalOut(gate);
00014   time  = tm;       // 積算時間(ms)
00015 
00016   *CK = 0;
00017   *Gate = 0;
00018   *Range= 1;    // 高感度モード
00019   setWhite();
00020 }
00021 
00022 void ColorSensor::setWhite()
00023 {
00024   unsigned short RGB[3];
00025   getRGB(RGB);
00026   R = RGB[0];
00027   G = RGB[1];
00028   B = RGB[2];
00029 }
00030 
00031 ColorSensor::ColorSensor(void) {
00032   unsigned short RGB[3];
00033   double R, G, B, r,g,b;
00034 
00035   Dout  = new DigitalIn(p5);    // Dout
00036   Range = new DigitalOut(p6);   // Range
00037   CK    = new DigitalOut(p7);   // CK
00038   Gate  = new DigitalOut(p8);   // Gate
00039 
00040   *CK   = 0; //ck
00041   *Gate = 0; //gate
00042   *Range = 1;  // 高感度モード range
00043   time = 10;   // 10ms
00044   setWhite();
00045 }
00046 
00047 void ColorSensor::getRGB(unsigned short RGB[])
00048 {
00049     unsigned short i, j, coldata;
00050 
00051      *Gate = 1; 
00052      wait_ms(time);      // 積算時間
00053      *Gate  = 0; 
00054      for(i=0; i<3; i++) {
00055        coldata=0;
00056        for(j=0; j<12; j++) {
00057          *CK = 1; 
00058          wait_us(1);
00059          coldata>>=1;
00060          if(*Dout) coldata|=0x800;
00061          *CK = 0;
00062          wait_us(1);
00063       }
00064       RGB[i]=coldata;
00065     }
00066 }
00067 
00068 void ColorSensor::getRGB(
00069   unsigned& R, unsigned& G, unsigned& B)
00070 {
00071     unsigned i, j, coldata;
00072 
00073      *Gate = 1; 
00074      wait_ms(time);       //  積算時間
00075      *Gate = 0;
00076      for(i=0; i<3; i++) {
00077        coldata=0;
00078        for(j=0; j<12; j++) {
00079          *CK = 1;
00080          wait_us(1);
00081          coldata>>=1;
00082          if(*Dout) coldata|=0x800;
00083          *CK = 0;
00084          wait_us(1);
00085        }
00086        switch(i) {
00087         case 0: R=coldata; break;
00088         case 1: G=coldata; break;
00089         case 2: B=coldata; break;
00090       }
00091     }
00092   }
00093 
00094   // 比率を%で返す
00095 unsigned ColorSensor::checkRGB(
00096   unsigned& R, unsigned& G, unsigned& B)
00097 {
00098      unsigned i, j, coldata, I;
00099 
00100      *Gate = 1; 
00101      wait_ms(time);      //  積算時間
00102      *Gate = 0;
00103      for(i=0; i<3; i++) {
00104        coldata=0;
00105        for(j=0; j<12; j++) {
00106          *CK = 1; 
00107          wait_us(1);
00108          coldata>>=1;
00109          if(*Dout) coldata|=0x800;
00110          *CK = false;
00111          wait_us(1);
00112        }
00113        switch(i) {
00114         case 0: R=coldata; break;
00115         case 1: G=coldata; break;
00116         case 2: B=coldata; break;
00117       }
00118     }
00119     I = R + G + B;
00120     R = R*100/I;
00121     G = G*100/I;
00122     B = B*100/I;
00123     return I;
00124 }
00125 
00126 int ColorSensor::judge()
00127 {
00128   unsigned short RGB[3];
00129   double r,g,b;
00130   *CK   = 0; //ck
00131   *Gate = 0; //gate
00132   *Range = 1;  // 高感度モード range
00133 
00134   getRGB(RGB);
00135   r = RGB[0]*100/R;
00136   g = RGB[1]*100/G;
00137   b = RGB[2]*100/B;
00138   if(r > g) {
00139        if(r > b*coef) return RED;
00140        else if(b > r*coef) return GREEN;
00141   }
00142    else {
00143      if(g > b*coef) return GREEN;
00144      else if(b > g*coef) return BLUE;
00145    }
00146   return OTHER;
00147 }