diff --git a/include/.gitkeep b/include/.gitkeep
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/include/Adafruit_TCS34725.h b/include/Adafruit_TCS34725.h
new file mode 100644
index 0000000000000000000000000000000000000000..46d58758bc0b683d246e32a45db09e26144f28cf
--- /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/lib/LiquidCrystal_I2C-1.1.3/LiquidCrystal_I2C.cpp b/lib/LiquidCrystal_I2C-1.1.3/LiquidCrystal_I2C.cpp
index 79f43894fefa3b7c0b3afe4530497baddebc23d8..87b1b09e0efab56abc76757e5e5848832e14dd94 100644
--- a/lib/LiquidCrystal_I2C-1.1.3/LiquidCrystal_I2C.cpp
+++ b/lib/LiquidCrystal_I2C-1.1.3/LiquidCrystal_I2C.cpp
@@ -58,7 +58,6 @@ void LiquidCrystal_I2C::init(){
void LiquidCrystal_I2C::init_priv()
{
- Wire.begin();
_displayfunction = LCD_4BITMODE | LCD_1LINE | LCD_5x8DOTS;
begin(_cols, _rows);
}
diff --git a/platformio.ini b/platformio.ini
index 4016204afb6833529c090aedab553bf3e28505c1..ba47ae6b9219b347873fe2ee7e6c84dd76bb2719 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -14,4 +14,5 @@ default_envs = nucleo_f303k8
[env:nucleo_f303k8]
platform = ststm32
framework = arduino
-board = nucleo_f303k8
\ No newline at end of file
+board = nucleo_f303k8
+monitor_speed = 57600
\ No newline at end of file
diff --git a/src/Adafruit_TCS34725.cpp b/src/Adafruit_TCS34725.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bb3a9e80695ed1119f7f52afe4467a1e73e5fe70
--- /dev/null
+++ b/src/Adafruit_TCS34725.cpp
@@ -0,0 +1,561 @@
+/*!
+ * @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
+
+// allow to divide delay time by this factor. Put the number of sensor here.
+// todo : make a better fix that don't use delay anymore.
+#define INTEG_DIVIDER 2
+
+#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/INTEG_DIVIDER) + 1);
+ break;
+ case TCS34725_INTEGRATIONTIME_24MS:
+ delay((24/INTEG_DIVIDER) + 1);
+ break;
+ case TCS34725_INTEGRATIONTIME_50MS:
+ delay((50/INTEG_DIVIDER) + 1);
+ break;
+ case TCS34725_INTEGRATIONTIME_101MS:
+ delay((101/INTEG_DIVIDER) + 1);
+ break;
+ case TCS34725_INTEGRATIONTIME_154MS:
+ delay((154/INTEG_DIVIDER) + 1);
+ break;
+ case TCS34725_INTEGRATIONTIME_700MS:
+ delay((700/INTEG_DIVIDER) + 1);
+ 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 63dd2c69970e81fd514a58fe9bd7b96a8e20de6e..c7541d5245a1d4c080213b2b44d77277d64fd06b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,18 +1,80 @@
+// TODO : créer un message pour les debugs de la carte d'alimentation (4 float)
+
#include <LiquidCrystal_I2C.h>
+#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>
+#include <ctrlpan_msgs/alim_stat.h>
+
+
+//#define DISABLE_COLOR 1
+#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;
+#define FLAG_UNFOLDED_ANGLE 195
+#define FLAG_FOLDED_ANGLE 20
+
+Servo armServo; //pour manche à air
+#define ARM_UNFOLDED_ANGLE 90
+#define ARM_FOLDED_ANGLE 180
+
+// I2C : (if not working : try inverse sda and scl pin)
+// todo : remove nucleo jumpers to use these pins.
+#define I2C_SDA A4
+#define I2C_SCL A5
// Pin declaration
-const int pullCordPin = D8;
-const int switchPin = D9;
-const int buttonPin = D10;
+const int flagServoPin = D3;
+const int armServoPin = D2;
+
+const int pullCordPin = A6;
+const int switchPin = D6;
+const int buttonPin = D12;
const int buttonLedPin = D11;
+const int voltage_twelvePin = A3;
+const int voltage_fivePin = A0;
+const int current_twelvePin = A2;
+const int current_fivePin = A1;
+#define VOLT12_RATIO 1
+#define VOLT5_RATIO 1
+#define CURR5_RATIO 1
+#define CURR12_RATIO 1
+
+const int bauStatusPin = D8;
+const int bauSoftPin = D9;
+
+
+// Flag state
+bool flagStatus;
+
+// BAU state
+bool bauSoftStatus;
+bool bauStatus;
+
+// Arm state
+bool armStatus;
+
+
+
// Current team (either TEAM_YELLOW or TEAM_BLUE)
bool team;
bool buttonStatus;
+bool buttonLedState;
// Last received estimated score
int estimatedScore = 0;
@@ -28,26 +90,58 @@ unsigned long lastDebounceTime = 0; // the last time the output pin was toggled
unsigned long debounceDelay = 50; // the debounce time; increase if the output flickers
unsigned long lastMessageTime = 0;
-unsigned int const MSG_SENDING_INTERVAL = 200;
+unsigned int const MSG_SENDING_INTERVAL = 100;
ros::NodeHandle nh;
std_msgs::Bool teamMsg;
std_msgs::Bool buttonStatusMsg;
std_msgs::Bool pullCordStatusMsg;
+std_msgs::Bool bauStatusMsg;
+ctrlpan_msgs::alim_stat alimStatMsg;
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);
+ros::Publisher alimStatPub("alim_stat", &alimStatMsg);
+
+
+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);
+void flagStatusChange(const std_msgs::Bool &flagStatusMsg);
+ros::Subscriber<std_msgs::Bool> flagStatusSub("flag_state", &flagStatusChange);
+
+void armStatusChange(const std_msgs::Bool &armStatusMsg);
+ros::Subscriber<std_msgs::Bool> armStateSub("arm_state", &armStatusChange);
+
+void bauSoftSet(const std_msgs::Bool &bauSoftSetMsg);
+ros::Subscriber<std_msgs::Bool> bauSoftSub("bau_state", &bauSoftSet);
+
+void buttonLedSet(const std_msgs::Bool &buttonLedSetMsg);
+ros::Subscriber<std_msgs::Bool> buttonLedSub("button_led_state", &buttonLedSet);
+
+
// LCD screen
LiquidCrystal_I2C lcd(0x27, 16, 2);
void refreshScreen()
{
+ //lcd.init();
+ //lcd.backlight();
+
lcd.clear();
lcd.setCursor(0, 0);
@@ -63,13 +157,48 @@ 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()
{
+
+ // INIT Wire on : A4 => SDA || A5 => SCL
+ Wire.begin();
+
+ flagServo.attach(flagServoPin);
+ flagServo.write(FLAG_FOLDED_ANGLE);
+
+ armServo.attach(armServoPin);
+ armServo.write(ARM_FOLDED_ANGLE);
+
pinMode(pullCordPin, INPUT_PULLUP);
pinMode(switchPin, INPUT_PULLUP);
pinMode(buttonPin, INPUT_PULLUP);
pinMode(buttonLedPin, OUTPUT);
+ pinMode(bauSoftPin, OUTPUT);
+ pinMode(bauStatusPin, INPUT);
+
digitalWrite(buttonLedPin, HIGH);
lcd.init();
@@ -79,10 +208,23 @@ void setup()
buttonStatus = digitalRead(buttonPin);
nh.initNode();
+
nh.advertise(teamStatusPub);
nh.advertise(buttonStatusPub);
nh.advertise(pullCordStatusPub);
+ nh.advertise(bauStatusPub);
+ nh.advertise(alimStatPub);
+
nh.subscribe(estimatedScoreSub);
+ nh.subscribe(flagStatusSub);
+ nh.subscribe(armStateSub);
+ nh.subscribe(bauSoftSub);
+ nh.subscribe(buttonLedSub);
+
+ // color Sensors setup
+ #ifndef DISABLE_COLOR
+ setupColorSensors();
+ #endif
lcd.clear();
lcd.setCursor(0, 0);
@@ -104,8 +246,117 @@ void estimatedScoreCallback(const std_msgs::Int32 &estimatedScoreMsg)
refreshScreen();
}
+// Appelé quand l'état du drapeau est mis à jour
+void flagStatusChange(const std_msgs::Bool &flagStatusMsg)
+{
+ flagStatus = flagStatusMsg.data;
+}
+
+// Appelé quand l'état du BAU est mis à jour
+void bauSoftSet(const std_msgs::Bool &bauSoftSetMsg)
+{
+ bauSoftStatus = bauSoftSetMsg.data;
+}
+
+// Appelé quand l'état du bras (manche à air) est mis à jour
+void armStatusChange(const std_msgs::Bool &armStatusMsg)
+{
+ armStatus = armStatusMsg.data;
+}
+
+// Appelé quand l'état de la led du bouton change
+void buttonLedSet(const std_msgs::Bool &buttonLedSetMsg)
+{
+ buttonLedState = buttonLedSetMsg.data;
+}
+
+void manage_supply_card_data(){
+
+ int volt12_valueADC = analogRead(voltage_twelvePin);
+ int volt5_valueADC = analogRead(voltage_fivePin);
+ int curr5_valueADC = analogRead(current_fivePin);
+ int curr12_valueADC = analogRead(current_twelvePin);
+
+ float volt12_value = (float) volt12_valueADC * VOLT12_RATIO;
+ float volt5_value = (float) volt5_valueADC * VOLT5_RATIO;
+ float curr5_value = (float) curr5_valueADC * CURR5_RATIO;
+ float curr12_value = (float) curr12_valueADC * CURR12_RATIO;
+
+ alimStatMsg.voltage_12 = volt12_value;
+ alimStatMsg.voltage_5 = volt5_value;
+ alimStatMsg.current_12 = curr12_value;
+ alimStatMsg.current_5 = curr5_value;
+
+}
+
+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;
+ 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 );
+ nh.spinOnce();
+}
+
void loop()
{
+ // TODOOOOOOO : CHANGE
if (!nh.connected())
{
@@ -122,6 +373,11 @@ void loop()
refreshScreen();
}
+
+ // color Sensors loop
+ #ifndef DISABLE_COLOR
+ colorSensorsLoop();
+ #endif
// DEBUT PARTIE TIRETTE
int pullCordReading = digitalRead(pullCordPin);
@@ -157,6 +413,7 @@ void loop()
refreshScreen();
}
+
if (digitalRead(switchPin) != team)
{
//Changement d'équipe
@@ -164,6 +421,9 @@ void loop()
refreshScreen();
}
+
+ bauStatus = digitalRead(bauStatusPin);
+
if (millis() - lastMessageTime >= MSG_SENDING_INTERVAL)
{
teamMsg.data = team;
@@ -175,7 +435,36 @@ void loop()
buttonStatusMsg.data = buttonStatus;
buttonStatusPub.publish(&buttonStatusMsg);
+ bauStatusMsg.data = bauStatus;
+ bauStatusPub.publish(&bauStatusMsg);
+
+ manage_supply_card_data();
+ alimStatPub.publish(&alimStatMsg);
+
lastMessageTime = millis();
nh.spinOnce();
}
-}
\ No newline at end of file
+
+ if(flagStatus)
+ {
+ flagServo.write(FLAG_UNFOLDED_ANGLE);
+ } else {
+ flagServo.write(FLAG_FOLDED_ANGLE);
+ }
+
+ if(armStatus)
+ {
+ armServo.write(ARM_UNFOLDED_ANGLE);
+ } else {
+ armServo.write(ARM_FOLDED_ANGLE);
+ }
+
+ if(buttonLedState)
+ {
+ digitalWrite(buttonLedPin, HIGH);
+ } else {
+ digitalWrite(buttonLedPin, LOW);
+ }
+
+ digitalWrite(bauSoftPin, bauSoftStatus);
+}