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;
00099      double I;
00100      
00101      *Gate = 1; 
00102      wait_ms(time);      //  積算時間
00103      *Gate = 0;
00104      for(i=0; i<3; i++) {
00105        coldata=0;
00106        for(j=0; j<12; j++) {
00107          *CK = 1; 
00108          wait_us(1);
00109          coldata>>=1;
00110          if(*Dout) coldata|=0x800;
00111          *CK = false;
00112          wait_us(1);
00113        }
00114        switch(i) {
00115         case 0: R=coldata; break;
00116         case 1: G=coldata; break;
00117         case 2: B=coldata; break;
00118       }
00119     }
00120     I = R*0.65 + G + B*1.3;
00121     R = R*0.65 *100/I;
00122     G = G*100/I;
00123     B = B*1.3 *100/I;
00124     return I;
00125 }
00126 
00127 int ColorSensor::judge()
00128 {
00129   unsigned short RGB[3];
00130   double r,g,b;
00131   *CK   = 0; //ck
00132   *Gate = 0; //gate
00133   *Range = 1;  // 高感度モード range
00134 
00135   getRGB(RGB);
00136   r = RGB[0]*100/R;
00137   g = RGB[1]*100/G;
00138   b = RGB[2]*100/B;
00139   if(r > g) {
00140        if(r > b*coef) return RED;
00141        else if(b > r*coef) return GREEN;
00142   }
00143    else {
00144      if(g > b*coef) return GREEN;
00145      else if(b > g*coef) return BLUE;
00146    }
00147   return OTHER;
00148 }