From 660c0568e5c049b8d5f00548ed8e5e0b684d639a Mon Sep 17 00:00:00 2001
From: thomas <thomas.vadebout@mailo.com>
Date: Sat, 3 Jul 2021 00:16:22 +0200
Subject: [PATCH] add color treatment

---
 src/main.cpp | 89 ++++++++++++++++++++++++++++++++++++++++++----------
 1 file changed, 73 insertions(+), 16 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index c7541d5..864e610 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,5 +1,8 @@
 // TODO : créer un message pour les debugs de la carte d'alimentation (4 float)
 
+#define SEND_HSV 1
+//#define DISABLE_COLOR 1
+
 #include <LiquidCrystal_I2C.h>
 
 #include <Servo.h>
@@ -10,18 +13,23 @@
 #include <ros.h>
 #include <std_msgs/Bool.h>
 #include <std_msgs/Int32.h>
-#include <geometry_msgs/Vector3.h>
+#include <colors_msgs/Colors.h>
 #include <ctrlpan_msgs/alim_stat.h>
 
+#ifdef SEND_HSV
+  #include <geometry_msgs/Vector3.h>
+#endif
+
 
-//#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;
-
+colors_msgs::Colors color_msg;
 
+#ifdef SEND_HSV
+  geometry_msgs::Vector3 hsv;
+#endif
 
 // Servo object declaration
 Servo flagServo; 
@@ -104,11 +112,18 @@ 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 colorSensorsPub("color_sensors", &color_msg);
 ros::Publisher alimStatPub("alim_stat", &alimStatMsg);
 
 
+#ifdef SEND_HSV
+ros::Publisher colorSensorsPub2("hsv_left", &hsv);
+ros::Publisher colorSensorsPub3("hsv_right", &hsv);
+#endif
+
+
+
+
 void TCA9548A(uint8_t bus)
 {
   Wire.beginTransmission(0x70);  // TCA9548A address is 0x70
@@ -162,7 +177,6 @@ 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");
@@ -173,10 +187,14 @@ void setupColorSensors(){
 
   if (tcs.begin()) {
     nh.loginfo("Found right color sensor");
-    nh.advertise(color_sensor_right);
   } else {
     nh.logerror("Right sensor not found");
   }
+  
+  nh.advertise(colorSensorsPub);
+  nh.advertise(colorSensorsPub2); // TO REMOVE
+  nh.advertise(colorSensorsPub3); // TO REMOVE
+
 }
 
 void setup()
@@ -326,6 +344,32 @@ int rgb_to_hsv(float& h, float& s,float& v, uint16_t re,uint16_t gr,uint16_t bl)
    return 0;
 }
 
+int traduceHSVtoInt(float h, float s, float v){
+
+  //WHITE
+  if (v < 6){
+    return color_msg.BLACK;
+  }
+
+  if (s < 18){
+    return color_msg.WHITE;
+  }
+
+  if (h > 350 || h < 10){
+    return color_msg.RED;
+  }
+
+  if (h > 20 && h < 40){
+    return color_msg.ORANGE;
+  }
+
+  if (h > 100 && h < 130){
+    return color_msg.GREEN;
+  }
+
+  return color_msg.UNKNOWN;
+}
+
 void colorSensorsLoop(){
   uint16_t r, g, b, c;
   float h, s, v;
@@ -334,23 +378,36 @@ void colorSensorsLoop(){
 
   tcs.getRawData(&r, &g, &b, &c);
   rgb_to_hsv(h, s, v, r, g, b);
+  int leftColor = traduceHSVtoInt(h, s, v);
 
-  hsv_msg.x = h;
-  hsv_msg.y = s;
-  hsv_msg.z = v;
+#ifdef SEND_HSV   
+    hsv.x = h;
+    hsv.y = s;
+    hsv.z = v;
 
-  color_sensor_left.publish( &hsv_msg );
+    colorSensorsPub2.publish( &hsv ); // to remove
+#endif
 
   TCA9548A(COL_R);
 
   tcs.getRawData(&r, &g, &b, &c);
   rgb_to_hsv(h, s, v, r, g, b);
+  int rightColor = traduceHSVtoInt(h, s, v);
+   
+#ifdef SEND_HSV   
+    hsv.x = h;
+    hsv.y = s;
+    hsv.z = v;
 
-  hsv_msg.x = h;
-  hsv_msg.y = s;
-  hsv_msg.z = v;
+    colorSensorsPub3.publish( &hsv ); // to remove
+#endif
+
+
+  color_msg.color_left = leftColor;
+  color_msg.color_right = rightColor;
+  
+  colorSensorsPub.publish( &color_msg );
 
-  color_sensor_right.publish( &hsv_msg );
   nh.spinOnce();
 }
 
-- 
GitLab