pan

Dependents:   jumpROBO4_0

Committer:
OGA
Date:
Thu Sep 12 12:06:29 2013 +0000
Revision:
0:c5bc72734cbf
jum

Who changed what in which revision?

UserRevisionLine numberNew 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 }