pan
ColorSensor.cpp@0:c5bc72734cbf, 2013-09-12 (annotated)
- Committer:
- OGA
- Date:
- Thu Sep 12 12:06:29 2013 +0000
- Revision:
- 0:c5bc72734cbf
jum
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
OGA | 0:c5bc72734cbf | 1 | #include "ColorSensor.h" |
OGA | 0:c5bc72734cbf | 2 | // カラーセンサーテストプログラム |
OGA | 0:c5bc72734cbf | 3 | // 赤、緑、青を判定する。 |
OGA | 0:c5bc72734cbf | 4 | #define coef 1.05 |
OGA | 0:c5bc72734cbf | 5 | |
OGA | 0:c5bc72734cbf | 6 | ColorSensor::ColorSensor( |
OGA | 0:c5bc72734cbf | 7 | PinName dout, PinName range, |
OGA | 0:c5bc72734cbf | 8 | PinName ck, PinName gate, int tm) |
OGA | 0:c5bc72734cbf | 9 | { |
OGA | 0:c5bc72734cbf | 10 | Dout = new DigitalIn(dout); |
OGA | 0:c5bc72734cbf | 11 | Range = new DigitalOut(range); |
OGA | 0:c5bc72734cbf | 12 | CK = new DigitalOut(ck); |
OGA | 0:c5bc72734cbf | 13 | Gate = new DigitalOut(gate); |
OGA | 0:c5bc72734cbf | 14 | time = tm; // 積算時間(ms) |
OGA | 0:c5bc72734cbf | 15 | |
OGA | 0:c5bc72734cbf | 16 | *CK = 0; |
OGA | 0:c5bc72734cbf | 17 | *Gate = 0; |
OGA | 0:c5bc72734cbf | 18 | *Range= 1; // 高感度モード |
OGA | 0:c5bc72734cbf | 19 | setWhite(); |
OGA | 0:c5bc72734cbf | 20 | } |
OGA | 0:c5bc72734cbf | 21 | |
OGA | 0:c5bc72734cbf | 22 | void ColorSensor::setWhite() |
OGA | 0:c5bc72734cbf | 23 | { |
OGA | 0:c5bc72734cbf | 24 | unsigned short RGB[3]; |
OGA | 0:c5bc72734cbf | 25 | getRGB(RGB); |
OGA | 0:c5bc72734cbf | 26 | R = RGB[0]; |
OGA | 0:c5bc72734cbf | 27 | G = RGB[1]; |
OGA | 0:c5bc72734cbf | 28 | B = RGB[2]; |
OGA | 0:c5bc72734cbf | 29 | } |
OGA | 0:c5bc72734cbf | 30 | |
OGA | 0:c5bc72734cbf | 31 | ColorSensor::ColorSensor(void) { |
OGA | 0:c5bc72734cbf | 32 | unsigned short RGB[3]; |
OGA | 0:c5bc72734cbf | 33 | double R, G, B, r,g,b; |
OGA | 0:c5bc72734cbf | 34 | |
OGA | 0:c5bc72734cbf | 35 | Dout = new DigitalIn(p5); // Dout |
OGA | 0:c5bc72734cbf | 36 | Range = new DigitalOut(p6); // Range |
OGA | 0:c5bc72734cbf | 37 | CK = new DigitalOut(p7); // CK |
OGA | 0:c5bc72734cbf | 38 | Gate = new DigitalOut(p8); // Gate |
OGA | 0:c5bc72734cbf | 39 | |
OGA | 0:c5bc72734cbf | 40 | *CK = 0; //ck |
OGA | 0:c5bc72734cbf | 41 | *Gate = 0; //gate |
OGA | 0:c5bc72734cbf | 42 | *Range = 1; // 高感度モード range |
OGA | 0:c5bc72734cbf | 43 | time = 10; // 10ms |
OGA | 0:c5bc72734cbf | 44 | setWhite(); |
OGA | 0:c5bc72734cbf | 45 | } |
OGA | 0:c5bc72734cbf | 46 | |
OGA | 0:c5bc72734cbf | 47 | void ColorSensor::getRGB(unsigned short RGB[]) |
OGA | 0:c5bc72734cbf | 48 | { |
OGA | 0:c5bc72734cbf | 49 | unsigned short i, j, coldata; |
OGA | 0:c5bc72734cbf | 50 | |
OGA | 0:c5bc72734cbf | 51 | *Gate = 1; |
OGA | 0:c5bc72734cbf | 52 | wait_ms(time); // 積算時間 |
OGA | 0:c5bc72734cbf | 53 | *Gate = 0; |
OGA | 0:c5bc72734cbf | 54 | for(i=0; i<3; i++) { |
OGA | 0:c5bc72734cbf | 55 | coldata=0; |
OGA | 0:c5bc72734cbf | 56 | for(j=0; j<12; j++) { |
OGA | 0:c5bc72734cbf | 57 | *CK = 1; |
OGA | 0:c5bc72734cbf | 58 | wait_us(1); |
OGA | 0:c5bc72734cbf | 59 | coldata>>=1; |
OGA | 0:c5bc72734cbf | 60 | if(*Dout) coldata|=0x800; |
OGA | 0:c5bc72734cbf | 61 | *CK = 0; |
OGA | 0:c5bc72734cbf | 62 | wait_us(1); |
OGA | 0:c5bc72734cbf | 63 | } |
OGA | 0:c5bc72734cbf | 64 | RGB[i]=coldata; |
OGA | 0:c5bc72734cbf | 65 | } |
OGA | 0:c5bc72734cbf | 66 | } |
OGA | 0:c5bc72734cbf | 67 | |
OGA | 0:c5bc72734cbf | 68 | void ColorSensor::getRGB( |
OGA | 0:c5bc72734cbf | 69 | unsigned& R, unsigned& G, unsigned& B) |
OGA | 0:c5bc72734cbf | 70 | { |
OGA | 0:c5bc72734cbf | 71 | unsigned i, j, coldata; |
OGA | 0:c5bc72734cbf | 72 | |
OGA | 0:c5bc72734cbf | 73 | *Gate = 1; |
OGA | 0:c5bc72734cbf | 74 | wait_ms(time); // 積算時間 |
OGA | 0:c5bc72734cbf | 75 | *Gate = 0; |
OGA | 0:c5bc72734cbf | 76 | for(i=0; i<3; i++) { |
OGA | 0:c5bc72734cbf | 77 | coldata=0; |
OGA | 0:c5bc72734cbf | 78 | for(j=0; j<12; j++) { |
OGA | 0:c5bc72734cbf | 79 | *CK = 1; |
OGA | 0:c5bc72734cbf | 80 | wait_us(1); |
OGA | 0:c5bc72734cbf | 81 | coldata>>=1; |
OGA | 0:c5bc72734cbf | 82 | if(*Dout) coldata|=0x800; |
OGA | 0:c5bc72734cbf | 83 | *CK = 0; |
OGA | 0:c5bc72734cbf | 84 | wait_us(1); |
OGA | 0:c5bc72734cbf | 85 | } |
OGA | 0:c5bc72734cbf | 86 | switch(i) { |
OGA | 0:c5bc72734cbf | 87 | case 0: R=coldata; break; |
OGA | 0:c5bc72734cbf | 88 | case 1: G=coldata; break; |
OGA | 0:c5bc72734cbf | 89 | case 2: B=coldata; break; |
OGA | 0:c5bc72734cbf | 90 | } |
OGA | 0:c5bc72734cbf | 91 | } |
OGA | 0:c5bc72734cbf | 92 | } |
OGA | 0:c5bc72734cbf | 93 | |
OGA | 0:c5bc72734cbf | 94 | // 比率を%で返す |
OGA | 0:c5bc72734cbf | 95 | //少し変更しました(2013/8) |
OGA | 0:c5bc72734cbf | 96 | unsigned ColorSensor::checkRGB( |
OGA | 0:c5bc72734cbf | 97 | unsigned& R, unsigned& G, unsigned& B) |
OGA | 0:c5bc72734cbf | 98 | { |
OGA | 0:c5bc72734cbf | 99 | unsigned i, j, coldata; |
OGA | 0:c5bc72734cbf | 100 | double I; |
OGA | 0:c5bc72734cbf | 101 | |
OGA | 0:c5bc72734cbf | 102 | *Gate = 1; |
OGA | 0:c5bc72734cbf | 103 | wait_ms(time); // 積算時間 |
OGA | 0:c5bc72734cbf | 104 | *Gate = 0; |
OGA | 0:c5bc72734cbf | 105 | for(i=0; i<3; i++) { |
OGA | 0:c5bc72734cbf | 106 | coldata=0; |
OGA | 0:c5bc72734cbf | 107 | for(j=0; j<12; j++) { |
OGA | 0:c5bc72734cbf | 108 | *CK = 1; |
OGA | 0:c5bc72734cbf | 109 | wait_us(1); |
OGA | 0:c5bc72734cbf | 110 | coldata>>=1; |
OGA | 0:c5bc72734cbf | 111 | if(*Dout) coldata|=0x800; |
OGA | 0:c5bc72734cbf | 112 | *CK = false; |
OGA | 0:c5bc72734cbf | 113 | wait_us(1); |
OGA | 0:c5bc72734cbf | 114 | } |
OGA | 0:c5bc72734cbf | 115 | switch(i) { |
OGA | 0:c5bc72734cbf | 116 | case 0: R=coldata; break; |
OGA | 0:c5bc72734cbf | 117 | case 1: G=coldata; break; |
OGA | 0:c5bc72734cbf | 118 | case 2: B=coldata; break; |
OGA | 0:c5bc72734cbf | 119 | } |
OGA | 0:c5bc72734cbf | 120 | } |
OGA | 0:c5bc72734cbf | 121 | I = R*0.65 + G + B*1.3; |
OGA | 0:c5bc72734cbf | 122 | R = R*0.65 *100/I; |
OGA | 0:c5bc72734cbf | 123 | G = G*100/I; |
OGA | 0:c5bc72734cbf | 124 | B = B*1.3 *100/I; |
OGA | 0:c5bc72734cbf | 125 | return I; |
OGA | 0:c5bc72734cbf | 126 | } |
OGA | 0:c5bc72734cbf | 127 | |
OGA | 0:c5bc72734cbf | 128 | int ColorSensor::judge() |
OGA | 0:c5bc72734cbf | 129 | { |
OGA | 0:c5bc72734cbf | 130 | unsigned short RGB[3]; |
OGA | 0:c5bc72734cbf | 131 | double r,g,b; |
OGA | 0:c5bc72734cbf | 132 | *CK = 0; //ck |
OGA | 0:c5bc72734cbf | 133 | *Gate = 0; //gate |
OGA | 0:c5bc72734cbf | 134 | *Range = 1; // 高感度モード range |
OGA | 0:c5bc72734cbf | 135 | |
OGA | 0:c5bc72734cbf | 136 | getRGB(RGB); |
OGA | 0:c5bc72734cbf | 137 | r = RGB[0]*100/R; |
OGA | 0:c5bc72734cbf | 138 | g = RGB[1]*100/G; |
OGA | 0:c5bc72734cbf | 139 | b = RGB[2]*100/B; |
OGA | 0:c5bc72734cbf | 140 | if(r > g) { |
OGA | 0:c5bc72734cbf | 141 | if(r > b*coef) return RED; |
OGA | 0:c5bc72734cbf | 142 | else if(b > r*coef) return GREEN; |
OGA | 0:c5bc72734cbf | 143 | } |
OGA | 0:c5bc72734cbf | 144 | else { |
OGA | 0:c5bc72734cbf | 145 | if(g > b*coef) return GREEN; |
OGA | 0:c5bc72734cbf | 146 | else if(b > g*coef) return BLUE; |
OGA | 0:c5bc72734cbf | 147 | } |
OGA | 0:c5bc72734cbf | 148 | return OTHER; |
OGA | 0:c5bc72734cbf | 149 | } |