aiu
Dependents: jumpROBO4_3 jumpROBO5_0 jumpROBO5_2 TX_Coloers ... more
Diff: ColorSensor.cpp
- Revision:
- 0:745c9de82674
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ColorSensor.cpp Mon Sep 23 06:30:00 2013 +0000 @@ -0,0 +1,154 @@ +#include "ColorSensor.h" +// カラーセンサーテストプログラム +// 赤、緑、青を判定する。 +#define coef 1.05 + +ColorSensor::ColorSensor( + PinName dout, PinName range, + PinName ck, PinName gate, int tm) +{ + Dout = new DigitalIn(dout); + Range = new DigitalOut(range); + CK = new DigitalOut(ck); + Gate = new DigitalOut(gate); + time = tm; // 積算時間(ms) + + *CK = 0; + *Gate = 0; + *Range= 1; // 高感度モード + setWhite(); +} + +void ColorSensor::setWhite() +{ + unsigned short RGB[3]; + getRGB(RGB); + R = RGB[0]; + G = RGB[1]; + B = RGB[2]; +} + +ColorSensor::ColorSensor(void) { + unsigned short RGB[3]; + double R, G, B, r,g,b; + + Dout = new DigitalIn(p5); // Dout + Range = new DigitalOut(p6); // Range + CK = new DigitalOut(p7); // CK + Gate = new DigitalOut(p8); // Gate + + *CK = 0; //ck + *Gate = 0; //gate + *Range = 1; // 高感度モード range + time = 10; // 10ms + setWhite(); +} + +void ColorSensor::getRGB(unsigned short RGB[]) +{ + unsigned short i, j, coldata; + + *Gate = 1; + wait_ms(time); // 積算時間 + *Gate = 0; + for(i=0; i<3; i++) { + coldata=0; + for(j=0; j<12; j++) { + *CK = 1; + wait_us(1); + coldata>>=1; + if(*Dout) coldata|=0x800; + *CK = 0; + wait_us(1); + } + RGB[i]=coldata; + } +} + +void ColorSensor::getRGB( + unsigned& R, unsigned& G, unsigned& B) +{ + unsigned i, j, coldata; + + *Gate = 1; + wait_ms(time); // 積算時間 + *Gate = 0; + for(i=0; i<3; i++) { + coldata=0; + for(j=0; j<12; j++) { + *CK = 1; + wait_us(1); + coldata>>=1; + if(*Dout) coldata|=0x800; + *CK = 0; + wait_us(1); + } + switch(i) { + case 0: R=coldata; break; + case 1: G=coldata; break; + case 2: B=coldata; break; + } + } + } + +// 比率を%で返す +//少し変更しました(2013/8) +unsigned ColorSensor::checkRGB( + unsigned& R, unsigned& G, unsigned& B) +{ + unsigned i, j, coldata; + double I; + + *Gate = 1; + wait_ms(time); // 積算時間 + *Gate = 0; + for(i=0; i<3; i++) { + coldata=0; + for(j=0; j<12; j++) { + *CK = 1; + wait_us(1); + coldata>>=1; + if(*Dout) coldata|=0x800; + *CK = false; + wait_us(1); + } + switch(i) { + case 0: R=coldata; break; + case 1: G=coldata; break; + case 2: B=coldata; break; + } + } + /*I = R*0.65 + G + B*1.3; + R = R*0.65 *100/I; + G = G*100/I; + B = B*1.3 *100/I;*/ + + I = R + G + B; + R = R*100/I; + G = G*100/I; + B = B*100/I; + return I; +} + +int ColorSensor::judge() +{ + unsigned short RGB[3]; + double r,g,b; + *CK = 0; //ck + *Gate = 0; //gate + *Range = 1; // 高感度モード range + + getRGB(RGB); + r = RGB[0]*100/R; + g = RGB[1]*100/G; + b = RGB[2]*100/B; + if(r > g) { + if(r > b*coef) return RED; + else if(b > r*coef) return GREEN; + } + else { + if(g > b*coef) return GREEN; + else if(b > g*coef) return BLUE; + } + return OTHER; +} \ No newline at end of file