Fumiya Fujisawa / ColorSensor

colorsensor +a

Dependents:   digtalcolorsensor

Fork of ColorSensor by Ryo Ogata

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?

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