#include "ColorSensor.h"
// カラーセンサーテストプログラム
// 赤、緑、青を判定する。
#define coef 1.05
ColorSensor::ColorSensor(
PinName dout, PinName range,
PinName ck, PinName gate, int tm)
{
Dout = new DigitalIn(dout);
Range = new DigitalOut(range);
CK = new DigitalOut(ck);
Gate = new DigitalOut(gate);
time = tm; // 積算時間(ms)
*CK = 0;
*Gate = 0;
*Range= 1; // 高感度モード
setWhite();
}
void ColorSensor::setWhite()
{
unsigned short RGB[3];
getRGB(RGB);
R = RGB[0];
G = RGB[1];
B = RGB[2];
}
ColorSensor::ColorSensor(void) {
unsigned short RGB[3];
double R, G, B, r,g,b;
Dout = new DigitalIn(p5); // Dout
Range = new DigitalOut(p6); // Range
CK = new DigitalOut(p7); // CK
Gate = new DigitalOut(p8); // Gate
*CK = 0; //ck
*Gate = 0; //gate
*Range = 1; // 高感度モード range
time = 10; // 10ms
setWhite();
}
void ColorSensor::getRGB(unsigned short RGB[])
{
unsigned short i, j, coldata;
*Gate = 1;
wait_ms(time); // 積算時間
*Gate = 0;
for(i=0; i<3; i++) {
coldata=0;
for(j=0; j<12; j++) {
*CK = 1;
wait_us(1);
coldata>>=1;
if(*Dout) coldata|=0x800;
*CK = 0;
wait_us(1);
}
RGB[i]=coldata;
}
}
void ColorSensor::getRGB(
unsigned& R, unsigned& G, unsigned& B)
{
unsigned i, j, coldata;
*Gate = 1;
wait_ms(time); // 積算時間
*Gate = 0;
for(i=0; i<3; i++) {
coldata=0;
for(j=0; j<12; j++) {
*CK = 1;
wait_us(1);
coldata>>=1;
if(*Dout) coldata|=0x800;
*CK = 0;
wait_us(1);
}
switch(i) {
case 0: R=coldata; break;
case 1: G=coldata; break;
case 2: B=coldata; break;
}
}
}
// 比率を%で返す
unsigned ColorSensor::checkRGB(
unsigned& R, unsigned& G, unsigned& B)
{
unsigned i, j, coldata, I;
*Gate = 1;
wait_ms(time); // 積算時間
*Gate = 0;
for(i=0; i<3; i++) {
coldata=0;
for(j=0; j<12; j++) {
*CK = 1;
wait_us(1);
coldata>>=1;
if(*Dout) coldata|=0x800;
*CK = false;
wait_us(1);
}
switch(i) {
case 0: R=coldata; break;
case 1: G=coldata; break;
case 2: B=coldata; break;
}
}
I = R + G + B;
R = R*100/I;
G = G*100/I;
B = B*100/I;
return I;
}
int ColorSensor::judge()
{
unsigned short RGB[3];
double r,g,b;
*CK = 0; //ck
*Gate = 0; //gate
*Range = 1; // 高感度モード range
getRGB(RGB);
r = RGB[0]*100/R;
g = RGB[1]*100/G;
b = RGB[2]*100/B;
if(r > g) {
if(r > b*coef) return RED;
else if(b > r*coef) return GREEN;
}
else {
if(g > b*coef) return GREEN;
else if(b > g*coef) return BLUE;
}
return OTHER;
}
|
1
2
3
4
5 色判定時の判定係数。実験による経験値
6
7
8
9
10 各ピンのオブジェクトを作り
11 その参照アドレスをインスタンス変数に
12 格納する
13
14 積算時間を格納する
15
16 クロックをLowにする
17 GateをLowにする
18 高感度モードにする
19 白レベルを計測する
20
21
22 白レベルを計測する
23
24 RGBの生データ一時保持
25 色計測
26 double型に変換し、保持
27
28
29
30
31 デフォールトのコンストラクタ
32
33
34
35 Doutはp5に接続する
36 Rangeはp6に接続する
37 CKはp7に接続する
38 Gateはp8に接続する
39
40 クロックをLowにする
41 GateをLowにする
42 高感度モードにする
43 計測時間を10msにする
44 白レベルを計測する
45
46
47 色データを取得する
48 配列に入れて返す
49
50
51 計測開始
52 光を積算する
53 計測終了
54 RGBの値をシリアルで取得する
55
56 各色データは12bit
57
58
59
60 LSBが先頭のデータ
61
62
63
64 色データを格納
65
66
67
68 参照型の変数に値を返す
69
70
71
72
73
74 光の積算はGATE信号をHighに行う
75
76 Red、Green、Blueの順に3回データを取得する
77
78 1つのデータは12bitである
79
80
81
82
83
84
85
86
87 1回目は赤
88 2回目は緑
89 3回目は青
90
91
92
93
94
95 RGBの比率を%で参照型で返す
96 光の強度を返す
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126 光の構成によって赤緑青に分類する
127
128
129
130
131
132
133
134
135 RはsetWhite()で設定した値
136 GはsetWhite()で設定した値
137 BはsetWhite()で設定した値
138
139
140
141
142
143
144
145
146
147
|
