From 916d6751fa11ed8945d6470c1d1069968087227e Mon Sep 17 00:00:00 2001
From: thomas <thomas.vadebout@mailo.com>
Date: Sun, 27 Jun 2021 22:55:26 +0200
Subject: [PATCH] add color sensors
---
include/Adafruit_TCS34725.h | 200 +++++++++++++
src/Adafruit_TCS34725.cpp | 556 ++++++++++++++++++++++++++++++++++++
src/main.cpp | 116 ++++++++
3 files changed, 872 insertions(+)
create mode 100644 include/Adafruit_TCS34725.h
create mode 100644 src/Adafruit_TCS34725.cpp
diff --git a/include/Adafruit_TCS34725.h b/include/Adafruit_TCS34725.h
new file mode 100644
index 0000000..46d5875
--- /dev/null
+++ b/include/Adafruit_TCS34725.h
@@ -0,0 +1,200 @@
+/*!
+ * @file Adafruit_TCS34725.h
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013, Adafruit Industries
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holders nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+ * DAMAGE.
+ */
+#ifndef _TCS34725_H_
+#define _TCS34725_H_
+
+#include <Arduino.h>
+
+#include <Wire.h>
+
+#define TCS34725_ADDRESS (0x29) /**< I2C address **/
+#define TCS34725_COMMAND_BIT (0x80) /**< Command bit **/
+#define TCS34725_ENABLE (0x00) /**< Interrupt Enable register */
+#define TCS34725_ENABLE_AIEN (0x10) /**< RGBC Interrupt Enable */
+#define TCS34725_ENABLE_WEN \
+ (0x08) /**< Wait Enable - Writing 1 activates the wait timer */
+#define TCS34725_ENABLE_AEN \
+ (0x02) /**< RGBC Enable - Writing 1 actives the ADC, 0 disables it */
+#define TCS34725_ENABLE_PON \
+ (0x01) /**< Power on - Writing 1 activates the internal oscillator, 0 \
+ disables it */
+#define TCS34725_ATIME (0x01) /**< Integration time */
+#define TCS34725_WTIME \
+ (0x03) /**< Wait time (if TCS34725_ENABLE_WEN is asserted) */
+#define TCS34725_WTIME_2_4MS (0xFF) /**< WLONG0 = 2.4ms WLONG1 = 0.029s */
+#define TCS34725_WTIME_204MS (0xAB) /**< WLONG0 = 204ms WLONG1 = 2.45s */
+#define TCS34725_WTIME_614MS (0x00) /**< WLONG0 = 614ms WLONG1 = 7.4s */
+#define TCS34725_AILTL \
+ (0x04) /**< Clear channel lower interrupt threshold (lower byte) */
+#define TCS34725_AILTH \
+ (0x05) /**< Clear channel lower interrupt threshold (higher byte) */
+#define TCS34725_AIHTL \
+ (0x06) /**< Clear channel upper interrupt threshold (lower byte) */
+#define TCS34725_AIHTH \
+ (0x07) /**< Clear channel upper interrupt threshold (higher byte) */
+#define TCS34725_PERS \
+ (0x0C) /**< Persistence register - basic SW filtering mechanism for \
+ interrupts */
+#define TCS34725_PERS_NONE \
+ (0b0000) /**< Every RGBC cycle generates an interrupt */
+#define TCS34725_PERS_1_CYCLE \
+ (0b0001) /**< 1 clean channel value outside threshold range generates an \
+ interrupt */
+#define TCS34725_PERS_2_CYCLE \
+ (0b0010) /**< 2 clean channel values outside threshold range generates an \
+ interrupt */
+#define TCS34725_PERS_3_CYCLE \
+ (0b0011) /**< 3 clean channel values outside threshold range generates an \
+ interrupt */
+#define TCS34725_PERS_5_CYCLE \
+ (0b0100) /**< 5 clean channel values outside threshold range generates an \
+ interrupt */
+#define TCS34725_PERS_10_CYCLE \
+ (0b0101) /**< 10 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_PERS_15_CYCLE \
+ (0b0110) /**< 15 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_PERS_20_CYCLE \
+ (0b0111) /**< 20 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_PERS_25_CYCLE \
+ (0b1000) /**< 25 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_PERS_30_CYCLE \
+ (0b1001) /**< 30 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_PERS_35_CYCLE \
+ (0b1010) /**< 35 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_PERS_40_CYCLE \
+ (0b1011) /**< 40 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_PERS_45_CYCLE \
+ (0b1100) /**< 45 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_PERS_50_CYCLE \
+ (0b1101) /**< 50 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_PERS_55_CYCLE \
+ (0b1110) /**< 55 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_PERS_60_CYCLE \
+ (0b1111) /**< 60 clean channel values outside threshold range generates an \
+ interrupt*/
+#define TCS34725_CONFIG (0x0D) /**< Configuration **/
+#define TCS34725_CONFIG_WLONG \
+ (0x02) /**< Choose between short and long (12x) wait times via \
+ TCS34725_WTIME */
+#define TCS34725_CONTROL (0x0F) /**< Set the gain level for the sensor */
+#define TCS34725_ID \
+ (0x12) /**< 0x44 = TCS34721/TCS34725, 0x4D = TCS34723/TCS34727 */
+#define TCS34725_STATUS (0x13) /**< Device status **/
+#define TCS34725_STATUS_AINT (0x10) /**< RGBC Clean channel interrupt */
+#define TCS34725_STATUS_AVALID \
+ (0x01) /**< Indicates that the RGBC channels have completed an integration \
+ cycle */
+#define TCS34725_CDATAL (0x14) /**< Clear channel data low byte */
+#define TCS34725_CDATAH (0x15) /**< Clear channel data high byte */
+#define TCS34725_RDATAL (0x16) /**< Red channel data low byte */
+#define TCS34725_RDATAH (0x17) /**< Red channel data high byte */
+#define TCS34725_GDATAL (0x18) /**< Green channel data low byte */
+#define TCS34725_GDATAH (0x19) /**< Green channel data high byte */
+#define TCS34725_BDATAL (0x1A) /**< Blue channel data low byte */
+#define TCS34725_BDATAH (0x1B) /**< Blue channel data high byte */
+
+/** Integration time settings for TCS34725 */
+typedef enum {
+ TCS34725_INTEGRATIONTIME_2_4MS =
+ 0xFF, /**< 2.4ms - 1 cycle - Max Count: 1024 */
+ TCS34725_INTEGRATIONTIME_24MS =
+ 0xF6, /**< 24ms - 10 cycles - Max Count: 10240 */
+ TCS34725_INTEGRATIONTIME_50MS =
+ 0xEB, /**< 50ms - 20 cycles - Max Count: 20480 */
+ TCS34725_INTEGRATIONTIME_101MS =
+ 0xD5, /**< 101ms - 42 cycles - Max Count: 43008 */
+ TCS34725_INTEGRATIONTIME_154MS =
+ 0xC0, /**< 154ms - 64 cycles - Max Count: 65535 */
+ TCS34725_INTEGRATIONTIME_700MS =
+ 0x00 /**< 700ms - 256 cycles - Max Count: 65535 */
+} tcs34725IntegrationTime_t;
+
+/** Gain settings for TCS34725 */
+typedef enum {
+ TCS34725_GAIN_1X = 0x00, /**< No gain */
+ TCS34725_GAIN_4X = 0x01, /**< 4x gain */
+ TCS34725_GAIN_16X = 0x02, /**< 16x gain */
+ TCS34725_GAIN_60X = 0x03 /**< 60x gain */
+} tcs34725Gain_t;
+
+/*!
+ * @brief Class that stores state and functions for interacting with
+ * TCS34725 Color Sensor
+ */
+class Adafruit_TCS34725 {
+public:
+ Adafruit_TCS34725(tcs34725IntegrationTime_t = TCS34725_INTEGRATIONTIME_2_4MS,
+ tcs34725Gain_t = TCS34725_GAIN_1X);
+
+ boolean begin(uint8_t addr, TwoWire *theWire);
+ boolean begin(uint8_t addr);
+ boolean begin();
+ boolean init();
+
+ void setIntegrationTime(tcs34725IntegrationTime_t it);
+ void setGain(tcs34725Gain_t gain);
+ void getRawData(uint16_t *r, uint16_t *g, uint16_t *b, uint16_t *c);
+ void getRGB(float *r, float *g, float *b);
+ void getRawDataOneShot(uint16_t *r, uint16_t *g, uint16_t *b, uint16_t *c);
+ uint16_t calculateColorTemperature(uint16_t r, uint16_t g, uint16_t b);
+ uint16_t calculateColorTemperature_dn40(uint16_t r, uint16_t g, uint16_t b,
+ uint16_t c);
+ uint16_t calculateLux(uint16_t r, uint16_t g, uint16_t b);
+ void write8(uint8_t reg, uint32_t value);
+ uint8_t read8(uint8_t reg);
+ uint16_t read16(uint8_t reg);
+ void setInterrupt(boolean flag);
+ void clearInterrupt();
+ void setIntLimits(uint16_t l, uint16_t h);
+ void enable();
+ void disable();
+
+private:
+ TwoWire *_wire;
+ uint8_t _i2caddr;
+ boolean _tcs34725Initialised;
+ tcs34725Gain_t _tcs34725Gain;
+ tcs34725IntegrationTime_t _tcs34725IntegrationTime;
+};
+
+#endif
diff --git a/src/Adafruit_TCS34725.cpp b/src/Adafruit_TCS34725.cpp
new file mode 100644
index 0000000..4438113
--- /dev/null
+++ b/src/Adafruit_TCS34725.cpp
@@ -0,0 +1,556 @@
+/*!
+ * @file Adafruit_TCS34725.cpp
+ *
+ * @mainpage Driver for the TCS34725 digital color sensors.
+ *
+ * @section intro_sec Introduction
+ *
+ * Adafruit invests time and resources providing this open source code,
+ * please support Adafruit and open-source hardware by purchasing
+ * products from Adafruit!
+ *
+ * @section author Author
+ *
+ * KTOWN (Adafruit Industries)
+ *
+ * @section license License
+ *
+ * BSD (see license.txt)
+ *
+ * @section HISTORY
+ *
+ * v1.0 - First release
+ */
+
+#define ARDUINO 200
+
+#include <math.h>
+#include <stdlib.h>
+
+#include "Adafruit_TCS34725.h"
+
+/*!
+ * @brief Implements missing powf function
+ * @param x
+ * Base number
+ * @param y
+ * Exponent
+ * @return x raised to the power of y
+ */
+float powf(const float x, const float y) {
+ return (float)(pow((double)x, (double)y));
+}
+
+/*!
+ * @brief Writes a register and an 8 bit value over I2C
+ * @param reg
+ * @param value
+ */
+void Adafruit_TCS34725::write8(uint8_t reg, uint32_t value) {
+ _wire->beginTransmission(_i2caddr);
+#if ARDUINO >= 100
+ _wire->write(TCS34725_COMMAND_BIT | reg);
+ _wire->write(value & 0xFF);
+#else
+ _wire->send(TCS34725_COMMAND_BIT | reg);
+ _wire->send(value & 0xFF);
+#endif
+ _wire->endTransmission();
+}
+
+/*!
+ * @brief Reads an 8 bit value over I2C
+ * @param reg
+ * @return value
+ */
+uint8_t Adafruit_TCS34725::read8(uint8_t reg) {
+ _wire->beginTransmission(_i2caddr);
+#if ARDUINO >= 100
+ _wire->write(TCS34725_COMMAND_BIT | reg);
+#else
+ _wire->send(TCS34725_COMMAND_BIT | reg);
+#endif
+ _wire->endTransmission();
+
+ _wire->requestFrom(_i2caddr, (uint8_t)1);
+#if ARDUINO >= 100
+ return _wire->read();
+#else
+ return _wire->receive();
+#endif
+}
+
+/*!
+ * @brief Reads a 16 bit values over I2C
+ * @param reg
+ * @return value
+ */
+uint16_t Adafruit_TCS34725::read16(uint8_t reg) {
+ uint16_t x;
+ uint16_t t;
+
+ _wire->beginTransmission(_i2caddr);
+#if ARDUINO >= 100
+ _wire->write(TCS34725_COMMAND_BIT | reg);
+#else
+ _wire->send(TCS34725_COMMAND_BIT | reg);
+#endif
+ _wire->endTransmission();
+
+ _wire->requestFrom(_i2caddr, (uint8_t)2);
+#if ARDUINO >= 100
+ t = _wire->read();
+ x = _wire->read();
+#else
+ t = _wire->receive();
+ x = _wire->receive();
+#endif
+ x <<= 8;
+ x |= t;
+ return x;
+}
+
+/*!
+ * @brief Enables the device
+ */
+void Adafruit_TCS34725::enable() {
+ write8(TCS34725_ENABLE, TCS34725_ENABLE_PON);
+ delay(3);
+ write8(TCS34725_ENABLE, TCS34725_ENABLE_PON | TCS34725_ENABLE_AEN);
+ /* Set a delay for the integration time.
+ This is only necessary in the case where enabling and then
+ immediately trying to read values back. This is because setting
+ AEN triggers an automatic integration, so if a read RGBC is
+ performed too quickly, the data is not yet valid and all 0's are
+ returned */
+ switch (_tcs34725IntegrationTime) {
+ case TCS34725_INTEGRATIONTIME_2_4MS:
+ delay(3);
+ break;
+ case TCS34725_INTEGRATIONTIME_24MS:
+ delay(24);
+ break;
+ case TCS34725_INTEGRATIONTIME_50MS:
+ delay(50);
+ break;
+ case TCS34725_INTEGRATIONTIME_101MS:
+ delay(101);
+ break;
+ case TCS34725_INTEGRATIONTIME_154MS:
+ delay(154);
+ break;
+ case TCS34725_INTEGRATIONTIME_700MS:
+ delay(700);
+ break;
+ }
+}
+
+/*!
+ * @brief Disables the device (putting it in lower power sleep mode)
+ */
+void Adafruit_TCS34725::disable() {
+ /* Turn the device off to save power */
+ uint8_t reg = 0;
+ reg = read8(TCS34725_ENABLE);
+ write8(TCS34725_ENABLE, reg & ~(TCS34725_ENABLE_PON | TCS34725_ENABLE_AEN));
+}
+
+/*!
+ * @brief Constructor
+ * @param it
+ * Integration Time
+ * @param gain
+ * Gain
+ */
+Adafruit_TCS34725::Adafruit_TCS34725(tcs34725IntegrationTime_t it,
+ tcs34725Gain_t gain) {
+ _tcs34725Initialised = false;
+ _tcs34725IntegrationTime = it;
+ _tcs34725Gain = gain;
+}
+
+/*!
+ * @brief Initializes I2C and configures the sensor
+ * @param addr
+ * i2c address
+ * @return True if initialization was successful, otherwise false.
+ */
+boolean Adafruit_TCS34725::begin(uint8_t addr) {
+ _i2caddr = addr;
+ _wire = &Wire;
+
+ return init();
+}
+
+/*!
+ * @brief Initializes I2C and configures the sensor
+ * @param addr
+ * i2c address
+ * @param *theWire
+ * The Wire object
+ * @return True if initialization was successful, otherwise false.
+ */
+boolean Adafruit_TCS34725::begin(uint8_t addr, TwoWire *theWire) {
+ _i2caddr = addr;
+ _wire = theWire;
+
+ return init();
+}
+
+/*!
+ * @brief Initializes I2C and configures the sensor
+ * @return True if initialization was successful, otherwise false.
+ */
+boolean Adafruit_TCS34725::begin() {
+ _i2caddr = TCS34725_ADDRESS;
+ _wire = &Wire;
+
+ return init();
+}
+
+/*!
+ * @brief Part of begin
+ * @return True if initialization was successful, otherwise false.
+ */
+boolean Adafruit_TCS34725::init() {
+ _wire->begin();
+
+ /* Make sure we're actually connected */
+ uint8_t x = read8(TCS34725_ID);
+ if ((x != 0x4d) && (x != 0x44) && (x != 0x10)) {
+ return false;
+ }
+ _tcs34725Initialised = true;
+
+ /* Set default integration time and gain */
+ setIntegrationTime(_tcs34725IntegrationTime);
+ setGain(_tcs34725Gain);
+
+ /* Note: by default, the device is in power down mode on bootup */
+ enable();
+
+ return true;
+}
+
+/*!
+ * @brief Sets the integration time for the TC34725
+ * @param it
+ * Integration Time
+ */
+void Adafruit_TCS34725::setIntegrationTime(tcs34725IntegrationTime_t it) {
+ if (!_tcs34725Initialised)
+ begin();
+
+ /* Update the timing register */
+ write8(TCS34725_ATIME, it);
+
+ /* Update value placeholders */
+ _tcs34725IntegrationTime = it;
+}
+
+/*!
+ * @brief Adjusts the gain on the TCS34725
+ * @param gain
+ * Gain (sensitivity to light)
+ */
+void Adafruit_TCS34725::setGain(tcs34725Gain_t gain) {
+ if (!_tcs34725Initialised)
+ begin();
+
+ /* Update the timing register */
+ write8(TCS34725_CONTROL, gain);
+
+ /* Update value placeholders */
+ _tcs34725Gain = gain;
+}
+
+/*!
+ * @brief Reads the raw red, green, blue and clear channel values
+ * @param *r
+ * Red value
+ * @param *g
+ * Green value
+ * @param *b
+ * Blue value
+ * @param *c
+ * Clear channel value
+ */
+void Adafruit_TCS34725::getRawData(uint16_t *r, uint16_t *g, uint16_t *b,
+ uint16_t *c) {
+ if (!_tcs34725Initialised)
+ begin();
+
+ *c = read16(TCS34725_CDATAL);
+ *r = read16(TCS34725_RDATAL);
+ *g = read16(TCS34725_GDATAL);
+ *b = read16(TCS34725_BDATAL);
+
+ /* Set a delay for the integration time */
+ switch (_tcs34725IntegrationTime) {
+ case TCS34725_INTEGRATIONTIME_2_4MS:
+ delay(3);
+ break;
+ case TCS34725_INTEGRATIONTIME_24MS:
+ delay(24);
+ break;
+ case TCS34725_INTEGRATIONTIME_50MS:
+ delay(50);
+ break;
+ case TCS34725_INTEGRATIONTIME_101MS:
+ delay(101);
+ break;
+ case TCS34725_INTEGRATIONTIME_154MS:
+ delay(154);
+ break;
+ case TCS34725_INTEGRATIONTIME_700MS:
+ delay(700);
+ break;
+ }
+}
+
+/*!
+ * @brief Reads the raw red, green, blue and clear channel values in
+ * one-shot mode (e.g., wakes from sleep, takes measurement, enters
+ * sleep)
+ * @param *r
+ * Red value
+ * @param *g
+ * Green value
+ * @param *b
+ * Blue value
+ * @param *c
+ * Clear channel value
+ */
+void Adafruit_TCS34725::getRawDataOneShot(uint16_t *r, uint16_t *g, uint16_t *b,
+ uint16_t *c) {
+ if (!_tcs34725Initialised)
+ begin();
+
+ enable();
+ getRawData(r, g, b, c);
+ disable();
+}
+
+/*!
+ * @brief Read the RGB color detected by the sensor.
+ * @param *r
+ * Red value normalized to 0-255
+ * @param *g
+ * Green value normalized to 0-255
+ * @param *b
+ * Blue value normalized to 0-255
+ */
+void Adafruit_TCS34725::getRGB(float *r, float *g, float *b) {
+ uint16_t red, green, blue, clear;
+ getRawData(&red, &green, &blue, &clear);
+ uint32_t sum = clear;
+
+ // Avoid divide by zero errors ... if clear = 0 return black
+ if (clear == 0) {
+ *r = *g = *b = 0;
+ return;
+ }
+
+ *r = (float)red / sum * 255.0;
+ *g = (float)green / sum * 255.0;
+ *b = (float)blue / sum * 255.0;
+}
+
+/*!
+ * @brief Converts the raw R/G/B values to color temperature in degrees Kelvin
+ * @param r
+ * Red value
+ * @param g
+ * Green value
+ * @param b
+ * Blue value
+ * @return Color temperature in degrees Kelvin
+ */
+uint16_t Adafruit_TCS34725::calculateColorTemperature(uint16_t r, uint16_t g,
+ uint16_t b) {
+ float X, Y, Z; /* RGB to XYZ correlation */
+ float xc, yc; /* Chromaticity co-ordinates */
+ float n; /* McCamy's formula */
+ float cct;
+
+ if (r == 0 && g == 0 && b == 0) {
+ return 0;
+ }
+
+ /* 1. Map RGB values to their XYZ counterparts. */
+ /* Based on 6500K fluorescent, 3000K fluorescent */
+ /* and 60W incandescent values for a wide range. */
+ /* Note: Y = Illuminance or lux */
+ X = (-0.14282F * r) + (1.54924F * g) + (-0.95641F * b);
+ Y = (-0.32466F * r) + (1.57837F * g) + (-0.73191F * b);
+ Z = (-0.68202F * r) + (0.77073F * g) + (0.56332F * b);
+
+ /* 2. Calculate the chromaticity co-ordinates */
+ xc = (X) / (X + Y + Z);
+ yc = (Y) / (X + Y + Z);
+
+ /* 3. Use McCamy's formula to determine the CCT */
+ n = (xc - 0.3320F) / (0.1858F - yc);
+
+ /* Calculate the final CCT */
+ cct =
+ (449.0F * powf(n, 3)) + (3525.0F * powf(n, 2)) + (6823.3F * n) + 5520.33F;
+
+ /* Return the results in degrees Kelvin */
+ return (uint16_t)cct;
+}
+
+/*!
+ * @brief Converts the raw R/G/B values to color temperature in degrees
+ * Kelvin using the algorithm described in DN40 from Taos (now AMS).
+ * @param r
+ * Red value
+ * @param g
+ * Green value
+ * @param b
+ * Blue value
+ * @param c
+ * Clear channel value
+ * @return Color temperature in degrees Kelvin
+ */
+uint16_t Adafruit_TCS34725::calculateColorTemperature_dn40(uint16_t r,
+ uint16_t g,
+ uint16_t b,
+ uint16_t c) {
+ uint16_t r2, b2; /* RGB values minus IR component */
+ uint16_t sat; /* Digital saturation level */
+ uint16_t ir; /* Inferred IR content */
+
+ if (c == 0) {
+ return 0;
+ }
+
+ /* Analog/Digital saturation:
+ *
+ * (a) As light becomes brighter, the clear channel will tend to
+ * saturate first since R+G+B is approximately equal to C.
+ * (b) The TCS34725 accumulates 1024 counts per 2.4ms of integration
+ * time, up to a maximum values of 65535. This means analog
+ * saturation can occur up to an integration time of 153.6ms
+ * (64*2.4ms=153.6ms).
+ * (c) If the integration time is > 153.6ms, digital saturation will
+ * occur before analog saturation. Digital saturation occurs when
+ * the count reaches 65535.
+ */
+ if ((256 - _tcs34725IntegrationTime) > 63) {
+ /* Track digital saturation */
+ sat = 65535;
+ } else {
+ /* Track analog saturation */
+ sat = 1024 * (256 - _tcs34725IntegrationTime);
+ }
+
+ /* Ripple rejection:
+ *
+ * (a) An integration time of 50ms or multiples of 50ms are required to
+ * reject both 50Hz and 60Hz ripple.
+ * (b) If an integration time faster than 50ms is required, you may need
+ * to average a number of samples over a 50ms period to reject ripple
+ * from fluorescent and incandescent light sources.
+ *
+ * Ripple saturation notes:
+ *
+ * (a) If there is ripple in the received signal, the value read from C
+ * will be less than the max, but still have some effects of being
+ * saturated. This means that you can be below the 'sat' value, but
+ * still be saturating. At integration times >150ms this can be
+ * ignored, but <= 150ms you should calculate the 75% saturation
+ * level to avoid this problem.
+ */
+ if ((256 - _tcs34725IntegrationTime) <= 63) {
+ /* Adjust sat to 75% to avoid analog saturation if atime < 153.6ms */
+ sat -= sat / 4;
+ }
+
+ /* Check for saturation and mark the sample as invalid if true */
+ if (c >= sat) {
+ return 0;
+ }
+
+ /* AMS RGB sensors have no IR channel, so the IR content must be */
+ /* calculated indirectly. */
+ ir = (r + g + b > c) ? (r + g + b - c) / 2 : 0;
+
+ /* Remove the IR component from the raw RGB values */
+ r2 = r - ir;
+ b2 = b - ir;
+
+ if (r2 == 0) {
+ return 0;
+ }
+
+ /* A simple method of measuring color temp is to use the ratio of blue */
+ /* to red light, taking IR cancellation into account. */
+ uint16_t cct = (3810 * (uint32_t)b2) / /** Color temp coefficient. */
+ (uint32_t)r2 +
+ 1391; /** Color temp offset. */
+
+ return cct;
+}
+
+/*!
+ * @brief Converts the raw R/G/B values to lux
+ * @param r
+ * Red value
+ * @param g
+ * Green value
+ * @param b
+ * Blue value
+ * @return Lux value
+ */
+uint16_t Adafruit_TCS34725::calculateLux(uint16_t r, uint16_t g, uint16_t b) {
+ float illuminance;
+
+ /* This only uses RGB ... how can we integrate clear or calculate lux */
+ /* based exclusively on clear since this might be more reliable? */
+ illuminance = (-0.32466F * r) + (1.57837F * g) + (-0.73191F * b);
+
+ return (uint16_t)illuminance;
+}
+
+/*!
+ * @brief Sets interrupt for TCS34725
+ * @param i
+ * Interrupt (True/False)
+ */
+void Adafruit_TCS34725::setInterrupt(boolean i) {
+ uint8_t r = read8(TCS34725_ENABLE);
+ if (i) {
+ r |= TCS34725_ENABLE_AIEN;
+ } else {
+ r &= ~TCS34725_ENABLE_AIEN;
+ }
+ write8(TCS34725_ENABLE, r);
+}
+
+/*!
+ * @brief Clears inerrupt for TCS34725
+ */
+void Adafruit_TCS34725::clearInterrupt() {
+ _wire->beginTransmission(_i2caddr);
+#if ARDUINO >= 100
+ _wire->write(TCS34725_COMMAND_BIT | 0x66);
+#else
+ _wire->send(TCS34725_COMMAND_BIT | 0x66);
+#endif
+ _wire->endTransmission();
+}
+
+/*!
+ * @brief Sets inerrupt limits
+ * @param low
+ * Low limit
+ * @param high
+ * High limit
+ */
+void Adafruit_TCS34725::setIntLimits(uint16_t low, uint16_t high) {
+ write8(0x04, low & 0xFF);
+ write8(0x05, low >> 8);
+ write8(0x06, high & 0xFF);
+ write8(0x07, high >> 8);
+}
diff --git a/src/main.cpp b/src/main.cpp
index d4a7f3e..0b81865 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -4,9 +4,23 @@
#include <Servo.h>
+#include "Adafruit_TCS34725.h"
+
+
+
+
#include <ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
+#include <geometry_msgs/Vector3.h>
+
+// color sensors
+#define COL_L 4
+#define COL_R 7
+Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_1X);
+ros::NodeHandle node_handle;
+geometry_msgs::Vector3 hsv_msg;
+
// Servo object declaration
Servo flagServo;
@@ -82,6 +96,17 @@ ros::Publisher teamStatusPub("team", &teamMsg);
ros::Publisher buttonStatusPub("button_status", &buttonStatusMsg);
ros::Publisher bauStatusPub("BAU_status", &bauStatusMsg);
ros::Publisher pullCordStatusPub("pull_cord_status", &pullCordStatusMsg);
+ros::Publisher color_sensor_left("color_sensor/left", &hsv_msg);
+ros::Publisher color_sensor_right("color_sensor/right", &hsv_msg);
+
+
+void TCA9548A(uint8_t bus)
+{
+ Wire.beginTransmission(0x70); // TCA9548A address is 0x70
+ Wire.write(1 << bus); // send byte to select bus
+ Wire.endTransmission();
+}
+
void estimatedScoreCallback(const std_msgs::Int32 &estimatedScore);
ros::Subscriber<std_msgs::Int32> estimatedScoreSub("estimated_score", &estimatedScoreCallback);
@@ -115,8 +140,28 @@ void refreshScreen()
lcd.print(buttonStatus ? "ON" : "OFF");
}
+void setupColorSensors(){
+
+ TCA9548A(COL_L);
+
+ if (tcs.begin()) {
+ nh.advertise(color_sensor_left);
+ nh.loginfo("Found left color sensor");
+ } else {
+ nh.logerror("Left sensor not found");
+ }
+ TCA9548A(COL_R);
+
+ if (tcs.begin()) {
+ nh.loginfo("Found right color sensor");
+ nh.advertise(color_sensor_right);
+ } else {
+ nh.logerror("Right sensor not found");
+ }
+}
+
void setup()
{
flagServo.attach(flagServoPin);
@@ -139,6 +184,7 @@ void setup()
buttonStatus = digitalRead(buttonPin);
nh.initNode();
+
nh.advertise(teamStatusPub);
nh.advertise(buttonStatusPub);
nh.advertise(pullCordStatusPub);
@@ -148,6 +194,9 @@ void setup()
nh.subscribe(flagStatusSub);
nh.subscribe(bauSoftSub);
+ // color Sensors setup
+ setupColorSensors();
+
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Waiting ROS...");
@@ -203,6 +252,69 @@ void manage_supply_card_data(){
}
+float max(float a, float b, float c) {
+ return ((a > b)? (a > c ? a : c) : (b > c ? b : c));
+}
+float min(float a, float b, float c) {
+ return ((a < b)? (a < c ? a : c) : (b < c ? b : c));
+}
+
+int rgb_to_hsv(float& h, float& s,float& v, uint16_t re,uint16_t gr,uint16_t bl){
+
+
+ float r = ((float) re) / (255.0);
+ float g = ((float) gr) / (255.0);
+ float b = ((float) bl) / (255.0);
+
+ float cmax = max(r, g, b); // maximum of r, g, b
+ float cmin = min(r, g, b); // minimum of r, g, b
+ float diff = cmax-cmin; // diff of cmax and cmin.
+
+ if (cmax == cmin)
+ h = 0;
+ else if (cmax == r)
+ h = fmod((60 * ((g - b) / diff) + 360), 360.0);
+ else if (cmax == g)
+ h = fmod((60 * ((b - r) / diff) + 120), 360.0);
+ else if (cmax == b)
+ h = fmod((60 * ((r - g) / diff) + 240), 360.0);
+ // if cmax equal zero
+ if (cmax == 0)
+ s = 0;
+ else
+ s = (diff / cmax) * 100;
+ // compute v
+ v = cmax * 100;
+
+ return 0;
+}
+
+void colorSensorsLoop(){
+ uint16_t r, g, b, c, colorTemp, lux;
+ float h, s, v;
+
+ TCA9548A(COL_L);
+
+ tcs.getRawData(&r, &g, &b, &c);
+ rgb_to_hsv(h, s, v, r, g, b);
+
+ hsv_msg.x = h;
+ hsv_msg.y = s;
+ hsv_msg.z = v;
+
+ color_sensor_left.publish( &hsv_msg );
+
+ TCA9548A(COL_R);
+
+ tcs.getRawData(&r, &g, &b, &c);
+ rgb_to_hsv(h, s, v, r, g, b);
+
+ hsv_msg.x = h;
+ hsv_msg.y = s;
+ hsv_msg.z = v;
+
+ color_sensor_right.publish( &hsv_msg );
+}
void loop()
{
@@ -222,6 +334,10 @@ void loop()
refreshScreen();
}
+
+ // color Sensors loop
+ colorSensorsLoop();
+
// DEBUT PARTIE TIRETTE
int pullCordReading = digitalRead(pullCordPin);
--
GitLab