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