Ryo Ogata / Mbed 2 deprecated jumpROBO3_5

oooga

Dependencies:   HMC6352 Servo TextLCD mbed

ColorSensor.cpp@0:e0e1b495278b, 2013-09-11 (annotated)

Committer:
OGA
Date:
Wed Sep 11 05:21:06 2013 +0000
Revision:
0:e0e1b495278b
ooo

Who changed what in which revision?

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