Ryo Ogata / ColorSensor1

aiu

Dependents:   jumpROBO4_3 jumpROBO5_0 jumpROBO5_2 TX_Coloers ... more

ColorSensor.cpp@0:745c9de82674, 2013-09-23 (annotated)

Committer:
OGA
Date:
Mon Sep 23 06:30:00 2013 +0000
Revision:
0:745c9de82674
colorsensor

Who changed what in which revision?

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