aiu
Dependents: jumpROBO4_3 jumpROBO5_0 jumpROBO5_2 TX_Coloers ... more
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 }
Generated on Wed Jul 20 2022 17:37:05 by
