yusuke takahashi / Mbed 2 deprecated colorSensor

tuika

Dependencies:   mbed

Fork of colorSensor by ryo seki

ColorSensor.cpp@1:86532e86498d, 2013-09-02 (annotated)

Committer:
yusuke_robocup
Date:
Mon Sep 02 13:09:42 2013 +0000
Revision:
1:86532e86498d
Parent:
0:7269a89e1d57 
tuika

Who changed what in which revision?

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