colorsensor +a
Fork of ColorSensor by
ColorSensor.cpp@1:51fc088353e7, 2013-09-19 (annotated)
- Committer:
- ryuna
- Date:
- Thu Sep 19 05:30:08 2013 +0000
- Revision:
- 1:51fc088353e7
- Parent:
- 0:c5bc72734cbf
colorsensor+a
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 | |
ryuna | 1:51fc088353e7 | 31 | void ColorSensor::timenow(unsigned short times){ |
ryuna | 1:51fc088353e7 | 32 | time = times ; |
ryuna | 1:51fc088353e7 | 33 | } |
OGA | 0:c5bc72734cbf | 34 | ColorSensor::ColorSensor(void) { |
OGA | 0:c5bc72734cbf | 35 | unsigned short RGB[3]; |
OGA | 0:c5bc72734cbf | 36 | double R, G, B, r,g,b; |
OGA | 0:c5bc72734cbf | 37 | |
OGA | 0:c5bc72734cbf | 38 | Dout = new DigitalIn(p5); // Dout |
OGA | 0:c5bc72734cbf | 39 | Range = new DigitalOut(p6); // Range |
OGA | 0:c5bc72734cbf | 40 | CK = new DigitalOut(p7); // CK |
OGA | 0:c5bc72734cbf | 41 | Gate = new DigitalOut(p8); // Gate |
OGA | 0:c5bc72734cbf | 42 | |
OGA | 0:c5bc72734cbf | 43 | *CK = 0; //ck |
OGA | 0:c5bc72734cbf | 44 | *Gate = 0; //gate |
OGA | 0:c5bc72734cbf | 45 | *Range = 1; // 高感度モード range |
OGA | 0:c5bc72734cbf | 46 | time = 10; // 10ms |
OGA | 0:c5bc72734cbf | 47 | setWhite(); |
OGA | 0:c5bc72734cbf | 48 | } |
ryuna | 1:51fc088353e7 | 49 | |
OGA | 0:c5bc72734cbf | 50 | void ColorSensor::getRGB(unsigned short RGB[]) |
OGA | 0:c5bc72734cbf | 51 | { |
OGA | 0:c5bc72734cbf | 52 | unsigned short i, j, coldata; |
OGA | 0:c5bc72734cbf | 53 | |
OGA | 0:c5bc72734cbf | 54 | *Gate = 1; |
OGA | 0:c5bc72734cbf | 55 | wait_ms(time); // 積算時間 |
OGA | 0:c5bc72734cbf | 56 | *Gate = 0; |
OGA | 0:c5bc72734cbf | 57 | for(i=0; i<3; i++) { |
OGA | 0:c5bc72734cbf | 58 | coldata=0; |
OGA | 0:c5bc72734cbf | 59 | for(j=0; j<12; j++) { |
OGA | 0:c5bc72734cbf | 60 | *CK = 1; |
OGA | 0:c5bc72734cbf | 61 | wait_us(1); |
OGA | 0:c5bc72734cbf | 62 | coldata>>=1; |
OGA | 0:c5bc72734cbf | 63 | if(*Dout) coldata|=0x800; |
OGA | 0:c5bc72734cbf | 64 | *CK = 0; |
OGA | 0:c5bc72734cbf | 65 | wait_us(1); |
OGA | 0:c5bc72734cbf | 66 | } |
OGA | 0:c5bc72734cbf | 67 | RGB[i]=coldata; |
OGA | 0:c5bc72734cbf | 68 | } |
OGA | 0:c5bc72734cbf | 69 | } |
ryuna | 1:51fc088353e7 | 70 | |
OGA | 0:c5bc72734cbf | 71 | void ColorSensor::getRGB( |
ryuna | 1:51fc088353e7 | 72 | unsigned& R, unsigned& G, unsigned& B ) |
OGA | 0:c5bc72734cbf | 73 | { |
OGA | 0:c5bc72734cbf | 74 | unsigned i, j, coldata; |
OGA | 0:c5bc72734cbf | 75 | |
OGA | 0:c5bc72734cbf | 76 | *Gate = 1; |
OGA | 0:c5bc72734cbf | 77 | wait_ms(time); // 積算時間 |
OGA | 0:c5bc72734cbf | 78 | *Gate = 0; |
OGA | 0:c5bc72734cbf | 79 | for(i=0; i<3; i++) { |
OGA | 0:c5bc72734cbf | 80 | coldata=0; |
OGA | 0:c5bc72734cbf | 81 | for(j=0; j<12; j++) { |
OGA | 0:c5bc72734cbf | 82 | *CK = 1; |
OGA | 0:c5bc72734cbf | 83 | wait_us(1); |
OGA | 0:c5bc72734cbf | 84 | coldata>>=1; |
OGA | 0:c5bc72734cbf | 85 | if(*Dout) coldata|=0x800; |
OGA | 0:c5bc72734cbf | 86 | *CK = 0; |
OGA | 0:c5bc72734cbf | 87 | wait_us(1); |
OGA | 0:c5bc72734cbf | 88 | } |
OGA | 0:c5bc72734cbf | 89 | switch(i) { |
OGA | 0:c5bc72734cbf | 90 | case 0: R=coldata; break; |
OGA | 0:c5bc72734cbf | 91 | case 1: G=coldata; break; |
OGA | 0:c5bc72734cbf | 92 | case 2: B=coldata; break; |
OGA | 0:c5bc72734cbf | 93 | } |
OGA | 0:c5bc72734cbf | 94 | } |
ryuna | 1:51fc088353e7 | 95 | } |
OGA | 0:c5bc72734cbf | 96 | |
OGA | 0:c5bc72734cbf | 97 | // 比率を%で返す |
OGA | 0:c5bc72734cbf | 98 | //少し変更しました(2013/8) |
ryuna | 1:51fc088353e7 | 99 | unsigned ColorSensor::checkRGB( |
ryuna | 1:51fc088353e7 | 100 | unsigned& R, unsigned& G, unsigned& B) |
OGA | 0:c5bc72734cbf | 101 | { |
OGA | 0:c5bc72734cbf | 102 | unsigned i, j, coldata; |
OGA | 0:c5bc72734cbf | 103 | double I; |
OGA | 0:c5bc72734cbf | 104 | |
OGA | 0:c5bc72734cbf | 105 | *Gate = 1; |
OGA | 0:c5bc72734cbf | 106 | wait_ms(time); // 積算時間 |
OGA | 0:c5bc72734cbf | 107 | *Gate = 0; |
OGA | 0:c5bc72734cbf | 108 | for(i=0; i<3; i++) { |
OGA | 0:c5bc72734cbf | 109 | coldata=0; |
OGA | 0:c5bc72734cbf | 110 | for(j=0; j<12; j++) { |
OGA | 0:c5bc72734cbf | 111 | *CK = 1; |
OGA | 0:c5bc72734cbf | 112 | wait_us(1); |
OGA | 0:c5bc72734cbf | 113 | coldata>>=1; |
OGA | 0:c5bc72734cbf | 114 | if(*Dout) coldata|=0x800; |
OGA | 0:c5bc72734cbf | 115 | *CK = false; |
OGA | 0:c5bc72734cbf | 116 | wait_us(1); |
OGA | 0:c5bc72734cbf | 117 | } |
OGA | 0:c5bc72734cbf | 118 | switch(i) { |
OGA | 0:c5bc72734cbf | 119 | case 0: R=coldata; break; |
OGA | 0:c5bc72734cbf | 120 | case 1: G=coldata; break; |
OGA | 0:c5bc72734cbf | 121 | case 2: B=coldata; break; |
OGA | 0:c5bc72734cbf | 122 | } |
OGA | 0:c5bc72734cbf | 123 | } |
OGA | 0:c5bc72734cbf | 124 | I = R*0.65 + G + B*1.3; |
ryuna | 1:51fc088353e7 | 125 | R = R*0.65*100/I; |
OGA | 0:c5bc72734cbf | 126 | G = G*100/I; |
ryuna | 1:51fc088353e7 | 127 | B = B*1.3*100/I; |
OGA | 0:c5bc72734cbf | 128 | return I; |
OGA | 0:c5bc72734cbf | 129 | } |
OGA | 0:c5bc72734cbf | 130 | |
OGA | 0:c5bc72734cbf | 131 | int ColorSensor::judge() |
OGA | 0:c5bc72734cbf | 132 | { |
OGA | 0:c5bc72734cbf | 133 | unsigned short RGB[3]; |
OGA | 0:c5bc72734cbf | 134 | double r,g,b; |
OGA | 0:c5bc72734cbf | 135 | *CK = 0; //ck |
OGA | 0:c5bc72734cbf | 136 | *Gate = 0; //gate |
OGA | 0:c5bc72734cbf | 137 | *Range = 1; // 高感度モード range |
OGA | 0:c5bc72734cbf | 138 | |
OGA | 0:c5bc72734cbf | 139 | getRGB(RGB); |
OGA | 0:c5bc72734cbf | 140 | r = RGB[0]*100/R; |
OGA | 0:c5bc72734cbf | 141 | g = RGB[1]*100/G; |
OGA | 0:c5bc72734cbf | 142 | b = RGB[2]*100/B; |
OGA | 0:c5bc72734cbf | 143 | if(r > g) { |
OGA | 0:c5bc72734cbf | 144 | if(r > b*coef) return RED; |
OGA | 0:c5bc72734cbf | 145 | else if(b > r*coef) return GREEN; |
OGA | 0:c5bc72734cbf | 146 | } |
OGA | 0:c5bc72734cbf | 147 | else { |
OGA | 0:c5bc72734cbf | 148 | if(g > b*coef) return GREEN; |
OGA | 0:c5bc72734cbf | 149 | else if(b > g*coef) return BLUE; |
OGA | 0:c5bc72734cbf | 150 | } |
OGA | 0:c5bc72734cbf | 151 | return OTHER; |
OGA | 0:c5bc72734cbf | 152 | } |