Carla Yoyotte / Mbed 2 deprecated Mon_Jeu

hy

Dependencies:   mbed

Classes/tColorSensor.cpp@0:daa4f58b511e, 2016-03-24 (annotated)

Committer:
CarlaY
Date:
Thu Mar 24 16:03:15 2016 +0000
Revision:
0:daa4f58b511e
ko;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CarlaY 0:daa4f58b511e 1 #include "tColorSensor.h"
CarlaY 0:daa4f58b511e 2 //#include <Wire.h>
CarlaY 0:daa4f58b511e 3 //#include <math.h>
CarlaY 0:daa4f58b511e 4
CarlaY 0:daa4f58b511e 5 // Default constructor
CarlaY 0:daa4f58b511e 6 tColorSensor::tColorSensor() : triggerMode_(INTEG_MODE_FREE | INTEG_PARAM_PULSE_COUNT1),
CarlaY 0:daa4f58b511e 7 interruptSource_(INT_SOURCE_CLEAR),
CarlaY 0:daa4f58b511e 8 interruptMode_(INTR_LEVEL | INTR_PERSIST_EVERY),
CarlaY 0:daa4f58b511e 9 gainAndPrescaler_(GAIN_1 | PRESCALER_1),
CarlaY 0:daa4f58b511e 10 sensorAddress_(COLOR_SENSOR_ADDR))
CarlaY 0:daa4f58b511e 11 {
CarlaY 0:daa4f58b511e 12 GroveColorSensor::setTimingReg();
CarlaY 0:daa4f58b511e 13 GroveColorSensor::setInterruptSourceReg();
CarlaY 0:daa4f58b511e 14 GroveColorSensor::setInterruptControlReg();
CarlaY 0:daa4f58b511e 15 GroveColorSensor::setGain();
CarlaY 0:daa4f58b511e 16 GroveColorSensor::setEnableADC();
CarlaY 0:daa4f58b511e 17 }
CarlaY 0:daa4f58b511e 18
CarlaY 0:daa4f58b511e 19 // Constructor with parameters
CarlaY 0:daa4f58b511e 20 GroveColorSensor::GroveColorSensor(
CarlaY 0:daa4f58b511e 21 const int& triggerMode
CarlaY 0:daa4f58b511e 22 , const int& interruptSource
CarlaY 0:daa4f58b511e 23 , const int& interruptMode
CarlaY 0:daa4f58b511e 24 , const int& gainAndPrescaler
CarlaY 0:daa4f58b511e 25 , const int& sensorAddress)
CarlaY 0:daa4f58b511e 26 : triggerMode_(triggerMode)
CarlaY 0:daa4f58b511e 27 , interruptSource_(interruptSource)
CarlaY 0:daa4f58b511e 28 , interruptMode_(interruptMode)
CarlaY 0:daa4f58b511e 29 , gainAndPrescaler_(gainAndPrescaler)
CarlaY 0:daa4f58b511e 30 , sensorAddress_(sensorAddress)
CarlaY 0:daa4f58b511e 31 {}
CarlaY 0:daa4f58b511e 32
CarlaY 0:daa4f58b511e 33 void GroveColorSensor::setTimingReg()
CarlaY 0:daa4f58b511e 34 {
CarlaY 0:daa4f58b511e 35 Wire.beginTransmission(sensorAddress_);
CarlaY 0:daa4f58b511e 36 Wire.write(REG_TIMING);
CarlaY 0:daa4f58b511e 37 Wire.write(triggerMode_);
CarlaY 0:daa4f58b511e 38 Wire.endTransmission();
CarlaY 0:daa4f58b511e 39 delay(10);
CarlaY 0:daa4f58b511e 40 }
CarlaY 0:daa4f58b511e 41
CarlaY 0:daa4f58b511e 42 void GroveColorSensor::setInterruptSourceReg()
CarlaY 0:daa4f58b511e 43 {
CarlaY 0:daa4f58b511e 44 Wire.beginTransmission(sensorAddress_);
CarlaY 0:daa4f58b511e 45 Wire.write(REG_INT_SOURCE);
CarlaY 0:daa4f58b511e 46 Wire.write(interruptSource_);
CarlaY 0:daa4f58b511e 47 Wire.endTransmission();
CarlaY 0:daa4f58b511e 48 delay(10);
CarlaY 0:daa4f58b511e 49 }
CarlaY 0:daa4f58b511e 50
CarlaY 0:daa4f58b511e 51 void GroveColorSensor::setInterruptControlReg()
CarlaY 0:daa4f58b511e 52 {
CarlaY 0:daa4f58b511e 53 Wire.beginTransmission(sensorAddress_);
CarlaY 0:daa4f58b511e 54 Wire.write(REG_INT);
CarlaY 0:daa4f58b511e 55 Wire.write(interruptMode_);
CarlaY 0:daa4f58b511e 56 Wire.endTransmission();
CarlaY 0:daa4f58b511e 57 delay(10);
CarlaY 0:daa4f58b511e 58 }
CarlaY 0:daa4f58b511e 59
CarlaY 0:daa4f58b511e 60 void GroveColorSensor::setGain()
CarlaY 0:daa4f58b511e 61 {
CarlaY 0:daa4f58b511e 62 Wire.beginTransmission(sensorAddress_);
CarlaY 0:daa4f58b511e 63 Wire.write(REG_GAIN);
CarlaY 0:daa4f58b511e 64 Wire.write(gainAndPrescaler_);
CarlaY 0:daa4f58b511e 65 Wire.endTransmission();
CarlaY 0:daa4f58b511e 66 }
CarlaY 0:daa4f58b511e 67
CarlaY 0:daa4f58b511e 68 void GroveColorSensor::setEnableADC()
CarlaY 0:daa4f58b511e 69 {
CarlaY 0:daa4f58b511e 70 Wire.beginTransmission(sensorAddress_);
CarlaY 0:daa4f58b511e 71 Wire.write(REG_CTL);
CarlaY 0:daa4f58b511e 72 Wire.write(CTL_DAT_INIITIATE);
CarlaY 0:daa4f58b511e 73 Wire.endTransmission();
CarlaY 0:daa4f58b511e 74 delay(10);
CarlaY 0:daa4f58b511e 75 }
CarlaY 0:daa4f58b511e 76
CarlaY 0:daa4f58b511e 77 void GroveColorSensor::clearInterrupt()
CarlaY 0:daa4f58b511e 78 {
CarlaY 0:daa4f58b511e 79 Wire.beginTransmission(sensorAddress_);
CarlaY 0:daa4f58b511e 80 Wire.write(CLR_INT);
CarlaY 0:daa4f58b511e 81 Wire.endTransmission();
CarlaY 0:daa4f58b511e 82 }
CarlaY 0:daa4f58b511e 83
CarlaY 0:daa4f58b511e 84 void GroveColorSensor::readRGB()
CarlaY 0:daa4f58b511e 85 {
CarlaY 0:daa4f58b511e 86 Wire.beginTransmission(sensorAddress_);
CarlaY 0:daa4f58b511e 87 Wire.write(REG_BLOCK_READ);
CarlaY 0:daa4f58b511e 88 Wire.endTransmission();
CarlaY 0:daa4f58b511e 89
CarlaY 0:daa4f58b511e 90 Wire.beginTransmission(sensorAddress_);
CarlaY 0:daa4f58b511e 91 Wire.requestFrom(sensorAddress_, 8);
CarlaY 0:daa4f58b511e 92 delay(100);
CarlaY 0:daa4f58b511e 93
CarlaY 0:daa4f58b511e 94 // if two bytes were received
CarlaY 0:daa4f58b511e 95 if(8 <= Wire.available())
CarlaY 0:daa4f58b511e 96 {
CarlaY 0:daa4f58b511e 97 int i;
CarlaY 0:daa4f58b511e 98 for(i = 0; i < 8; ++i)
CarlaY 0:daa4f58b511e 99 {
CarlaY 0:daa4f58b511e 100 readingdata_[i] = Wire.read();
CarlaY 0:daa4f58b511e 101 //Serial.println(readingdata_[i], BIN);
CarlaY 0:daa4f58b511e 102 }
CarlaY 0:daa4f58b511e 103 }
CarlaY 0:daa4f58b511e 104 green_ = readingdata_[1] * 256 + readingdata_[0];
CarlaY 0:daa4f58b511e 105 red_ = readingdata_[3] * 256 + readingdata_[2];
CarlaY 0:daa4f58b511e 106 blue_ = readingdata_[5] * 256 + readingdata_[4];
CarlaY 0:daa4f58b511e 107 clear_ = readingdata_[7] * 256 + readingdata_[6];
CarlaY 0:daa4f58b511e 108
CarlaY 0:daa4f58b511e 109 Serial.print("The RGB value are: RGB( ");
CarlaY 0:daa4f58b511e 110 Serial.print(red_,DEC);
CarlaY 0:daa4f58b511e 111 Serial.print(", ");
CarlaY 0:daa4f58b511e 112 Serial.print(green_,DEC);
CarlaY 0:daa4f58b511e 113 Serial.print(", ");
CarlaY 0:daa4f58b511e 114 Serial.print(blue_,DEC);
CarlaY 0:daa4f58b511e 115 Serial.println(" )");
CarlaY 0:daa4f58b511e 116 Serial.print("The Clear channel value are: ");
CarlaY 0:daa4f58b511e 117 Serial.println(clear_,DEC);
CarlaY 0:daa4f58b511e 118 }
CarlaY 0:daa4f58b511e 119
CarlaY 0:daa4f58b511e 120 void GroveColorSensor::readRGB(int *red, int *green, int *blue)
CarlaY 0:daa4f58b511e 121 {
CarlaY 0:daa4f58b511e 122 Wire.beginTransmission(sensorAddress_);
CarlaY 0:daa4f58b511e 123 Wire.write(REG_BLOCK_READ);
CarlaY 0:daa4f58b511e 124 Wire.endTransmission();
CarlaY 0:daa4f58b511e 125
CarlaY 0:daa4f58b511e 126 Wire.beginTransmission(sensorAddress_);
CarlaY 0:daa4f58b511e 127 Wire.requestFrom(sensorAddress_, 8);
CarlaY 0:daa4f58b511e 128 delay(100);
CarlaY 0:daa4f58b511e 129
CarlaY 0:daa4f58b511e 130 // if two bytes were received
CarlaY 0:daa4f58b511e 131 if(8 <= Wire.available())
CarlaY 0:daa4f58b511e 132 {
CarlaY 0:daa4f58b511e 133 int i;
CarlaY 0:daa4f58b511e 134 for(i = 0; i < 8; ++i)
CarlaY 0:daa4f58b511e 135 {
CarlaY 0:daa4f58b511e 136 readingdata_[i] = Wire.read();
CarlaY 0:daa4f58b511e 137 //Serial.println(readingdata_[i], BIN);
CarlaY 0:daa4f58b511e 138 }
CarlaY 0:daa4f58b511e 139 }
CarlaY 0:daa4f58b511e 140 green_ = readingdata_[1] * 256 + readingdata_[0];
CarlaY 0:daa4f58b511e 141 red_ = readingdata_[3] * 256 + readingdata_[2];
CarlaY 0:daa4f58b511e 142 blue_ = readingdata_[5] * 256 + readingdata_[4];
CarlaY 0:daa4f58b511e 143 clear_ = readingdata_[7] * 256 + readingdata_[6];
CarlaY 0:daa4f58b511e 144
CarlaY 0:daa4f58b511e 145 double tmp;
CarlaY 0:daa4f58b511e 146 int maxColor;
CarlaY 0:daa4f58b511e 147
CarlaY 0:daa4f58b511e 148 if ( ledStatus == 1 )
CarlaY 0:daa4f58b511e 149 {
CarlaY 0:daa4f58b511e 150 red_ = red_ * 1.70;
CarlaY 0:daa4f58b511e 151 blue_ = blue_ * 1.35;
CarlaY 0:daa4f58b511e 152
CarlaY 0:daa4f58b511e 153 maxColor = max(red_, green_);
CarlaY 0:daa4f58b511e 154 maxColor = max(maxColor, blue_);
CarlaY 0:daa4f58b511e 155
CarlaY 0:daa4f58b511e 156 if(maxColor > 255)
CarlaY 0:daa4f58b511e 157 {
CarlaY 0:daa4f58b511e 158 tmp = 250.0/maxColor;
CarlaY 0:daa4f58b511e 159 green_ *= tmp;
CarlaY 0:daa4f58b511e 160 red_ *= tmp;
CarlaY 0:daa4f58b511e 161 blue_ *= tmp;
CarlaY 0:daa4f58b511e 162 }
CarlaY 0:daa4f58b511e 163 }
CarlaY 0:daa4f58b511e 164 if ( ledStatus == 0 )
CarlaY 0:daa4f58b511e 165 {
CarlaY 0:daa4f58b511e 166 maxColor = max(red_, green_);
CarlaY 0:daa4f58b511e 167 maxColor = max(maxColor, blue_);
CarlaY 0:daa4f58b511e 168
CarlaY 0:daa4f58b511e 169 tmp = 250.0/maxColor;
CarlaY 0:daa4f58b511e 170 green_ *= tmp;
CarlaY 0:daa4f58b511e 171 red_ *= tmp;
CarlaY 0:daa4f58b511e 172 blue_ *= tmp;
CarlaY 0:daa4f58b511e 173
CarlaY 0:daa4f58b511e 174 }
CarlaY 0:daa4f58b511e 175
CarlaY 0:daa4f58b511e 176 int minColor = min(red_, green_);
CarlaY 0:daa4f58b511e 177 minColor = min(maxColor, blue_);
CarlaY 0:daa4f58b511e 178 maxColor = max(red_, green_);
CarlaY 0:daa4f58b511e 179 maxColor = max(maxColor, blue_);
CarlaY 0:daa4f58b511e 180
CarlaY 0:daa4f58b511e 181 int greenTmp = green_;
CarlaY 0:daa4f58b511e 182 int redTmp = red_;
CarlaY 0:daa4f58b511e 183 int blueTmp = blue_;
CarlaY 0:daa4f58b511e 184
CarlaY 0:daa4f58b511e 185 //when turn on LED, need to adjust the RGB data,otherwise it is almost the white color
CarlaY 0:daa4f58b511e 186 if(red_ < 0.8*maxColor && red_ >= 0.6*maxColor)
CarlaY 0:daa4f58b511e 187 {
CarlaY 0:daa4f58b511e 188 red_ *= 0.4;
CarlaY 0:daa4f58b511e 189 }
CarlaY 0:daa4f58b511e 190 else if(red_ < 0.6*maxColor)
CarlaY 0:daa4f58b511e 191 {
CarlaY 0:daa4f58b511e 192 red_ *= 0.2;
CarlaY 0:daa4f58b511e 193 }
CarlaY 0:daa4f58b511e 194
CarlaY 0:daa4f58b511e 195 if(green_ < 0.8*maxColor && green_ >= 0.6*maxColor)
CarlaY 0:daa4f58b511e 196 {
CarlaY 0:daa4f58b511e 197 green_ *= 0.4;
CarlaY 0:daa4f58b511e 198 }
CarlaY 0:daa4f58b511e 199 else if(green_ < 0.6*maxColor)
CarlaY 0:daa4f58b511e 200 {
CarlaY 0:daa4f58b511e 201 if (maxColor == redTmp && greenTmp >= 2*blueTmp && greenTmp >= 0.2*redTmp) //orange
CarlaY 0:daa4f58b511e 202 {
CarlaY 0:daa4f58b511e 203 green_ *= 5;
CarlaY 0:daa4f58b511e 204 }
CarlaY 0:daa4f58b511e 205 green_ *= 0.2;
CarlaY 0:daa4f58b511e 206 }
CarlaY 0:daa4f58b511e 207
CarlaY 0:daa4f58b511e 208 if(blue_ < 0.8*maxColor && blue_ >= 0.6*maxColor)
CarlaY 0:daa4f58b511e 209 {
CarlaY 0:daa4f58b511e 210 blue_ *= 0.4;
CarlaY 0:daa4f58b511e 211 }
CarlaY 0:daa4f58b511e 212 else if(blue_ < 0.6*maxColor)
CarlaY 0:daa4f58b511e 213 {
CarlaY 0:daa4f58b511e 214 if (maxColor == redTmp && greenTmp >= 2*blueTmp && greenTmp >= 0.2*redTmp) //orange
CarlaY 0:daa4f58b511e 215 {
CarlaY 0:daa4f58b511e 216 blue_ *= 0.5;
CarlaY 0:daa4f58b511e 217 }
CarlaY 0:daa4f58b511e 218 if (maxColor == redTmp && greenTmp <= blueTmp && blueTmp >= 0.2*redTmp) //pink
CarlaY 0:daa4f58b511e 219 {
CarlaY 0:daa4f58b511e 220 blue_ *= 5;
CarlaY 0:daa4f58b511e 221 }
CarlaY 0:daa4f58b511e 222 blue_ *= 0.2;
CarlaY 0:daa4f58b511e 223 }
CarlaY 0:daa4f58b511e 224
CarlaY 0:daa4f58b511e 225 minColor = min(red_, green_);
CarlaY 0:daa4f58b511e 226 minColor = min(maxColor, blue_);
CarlaY 0:daa4f58b511e 227 if(maxColor == green_ && red_ >= 0.85*maxColor && minColor == blue_) //yellow
CarlaY 0:daa4f58b511e 228 {
CarlaY 0:daa4f58b511e 229 red_ = maxColor;
CarlaY 0:daa4f58b511e 230 blue_ *= 0.4;
CarlaY 0:daa4f58b511e 231 }
CarlaY 0:daa4f58b511e 232
CarlaY 0:daa4f58b511e 233 *red = red_;
CarlaY 0:daa4f58b511e 234 *green = green_;
CarlaY 0:daa4f58b511e 235 *blue = blue_;
CarlaY 0:daa4f58b511e 236 }
CarlaY 0:daa4f58b511e 237
CarlaY 0:daa4f58b511e 238 void GroveColorSensor::calculateCoordinate()
CarlaY 0:daa4f58b511e 239 {
CarlaY 0:daa4f58b511e 240 double X;
CarlaY 0:daa4f58b511e 241 double Y;
CarlaY 0:daa4f58b511e 242 double Z;
CarlaY 0:daa4f58b511e 243 double x;
CarlaY 0:daa4f58b511e 244 double y;
CarlaY 0:daa4f58b511e 245
CarlaY 0:daa4f58b511e 246 X = (-0.14282) * red_ + (1.54924) * green_ + (-0.95641) * blue_;
CarlaY 0:daa4f58b511e 247 Y = (-0.32466) * red_ + (1.57837) * green_ + (-0.73191) * blue_;
CarlaY 0:daa4f58b511e 248 Z = (-0.68202) * red_ + (0.77073) * green_ + (0.563320) * blue_;
CarlaY 0:daa4f58b511e 249
CarlaY 0:daa4f58b511e 250 x = X / (X + Y + Z);
CarlaY 0:daa4f58b511e 251 y = Y / (X + Y + Z);
CarlaY 0:daa4f58b511e 252
CarlaY 0:daa4f58b511e 253 if( (X > 0) && ( Y > 0) && ( Z > 0) )
CarlaY 0:daa4f58b511e 254 {
CarlaY 0:daa4f58b511e 255 Serial.println("The x,y values are(");
CarlaY 0:daa4f58b511e 256 Serial.print(x, 2);
CarlaY 0:daa4f58b511e 257 Serial.print(" , ");
CarlaY 0:daa4f58b511e 258 Serial.print(y, 2);
CarlaY 0:daa4f58b511e 259 Serial.println(")");
CarlaY 0:daa4f58b511e 260 }
CarlaY 0:daa4f58b511e 261 else
CarlaY 0:daa4f58b511e 262 Serial.println("Error: overflow!");
CarlaY 0:daa4f58b511e 263 }