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/main.cpp b/src/main.cpp
index eec42ad058a58457e31d77f6524f50c2524eb610..c7541d5245a1d4c080213b2b44d77277d64fd06b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -4,9 +4,7 @@
 
 #include <Servo.h>
 
-#include "Adafruit_TCS34725.h"
-
-
+#include <Adafruit_TCS34725.h>
 
 
 #include <ros.h>
@@ -16,7 +14,7 @@
 #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);
@@ -27,26 +25,26 @@ geometry_msgs::Vector3 hsv_msg;
 
 // Servo object declaration
 Servo flagServo; 
-#define FLAG_UNFOLDED_ANGLE 170
-#define FLAG_FOLDED_ANGLE 0
+#define FLAG_UNFOLDED_ANGLE 195
+#define FLAG_FOLDED_ANGLE 20
 
 Servo armServo; //pour manche à air
 #define ARM_UNFOLDED_ANGLE 90
-#define ARM_FOLDED_ANGLE 0
+#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 A5
-#define I2C_SCL A4
+#define I2C_SDA A4
+#define I2C_SCL A5
 
 // Pin declaration
-const int flagServoPin = D6;
-const int armServoPin = D4;
+const int flagServoPin = D3;
+const int armServoPin = D2;
 
 const int pullCordPin = A6;
-const int switchPin = A7;
-const int buttonPin = D10;
-const int buttonLedPin = D12;
+const int switchPin = D6;
+const int buttonPin = D12;
+const int buttonLedPin = D11;
 
 const int voltage_twelvePin = A3;
 const int voltage_fivePin = A0;
@@ -76,6 +74,7 @@ bool armStatus;
 // Current team (either TEAM_YELLOW or TEAM_BLUE)
 bool team;
 bool buttonStatus;
+bool buttonLedState;
 
 // Last received estimated score
 int estimatedScore = 0;
@@ -91,7 +90,7 @@ 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;
 
@@ -131,11 +130,18 @@ 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);
@@ -177,14 +183,13 @@ void setup()
 {
 
   // INIT Wire on : A4 => SDA  ||  A5 => SCL
-	Wire.begin(I2C_SDA, I2C_SCL);
-
+	Wire.begin();
 
   flagServo.attach(flagServoPin);
-  flagServo.write(0);
+  flagServo.write(FLAG_FOLDED_ANGLE);
 
   armServo.attach(armServoPin);
-  armServo.write(0);
+  armServo.write(ARM_FOLDED_ANGLE);
 
   pinMode(pullCordPin, INPUT_PULLUP);
   pinMode(switchPin, INPUT_PULLUP);
@@ -214,9 +219,12 @@ void setup()
   nh.subscribe(flagStatusSub);
   nh.subscribe(armStateSub);
   nh.subscribe(bauSoftSub);
+  nh.subscribe(buttonLedSub);
 
   // color Sensors setup
-  setupColorSensors();
+  #ifndef DISABLE_COLOR
+    setupColorSensors();
+  #endif
 
   lcd.clear();
   lcd.setCursor(0, 0);
@@ -256,7 +264,11 @@ 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(){
 
@@ -315,7 +327,7 @@ int rgb_to_hsv(float& h, float& s,float& v, uint16_t re,uint16_t gr,uint16_t bl)
 }
 
 void colorSensorsLoop(){
-  uint16_t r, g, b, c, colorTemp, lux;
+  uint16_t r, g, b, c;
   float h, s, v;
 
   TCA9548A(COL_L);
@@ -363,8 +375,9 @@ void loop()
 
 
   // color Sensors loop
-  colorSensorsLoop();
-
+  #ifndef DISABLE_COLOR
+    colorSensorsLoop();
+  #endif
   // DEBUT PARTIE TIRETTE
 
   int pullCordReading = digitalRead(pullCordPin);
@@ -400,6 +413,7 @@ void loop()
     refreshScreen();
   }
 
+
   if (digitalRead(switchPin) != team)
   {
     //Changement d'équipe
@@ -407,6 +421,7 @@ void loop()
     refreshScreen();
   }
 
+
   bauStatus = digitalRead(bauStatusPin);
 
   if (millis() - lastMessageTime >= MSG_SENDING_INTERVAL)
@@ -444,5 +459,12 @@ void loop()
     armServo.write(ARM_FOLDED_ANGLE);
   }
 
+  if(buttonLedState)
+  {
+    digitalWrite(buttonLedPin, HIGH);
+  } else {
+    digitalWrite(buttonLedPin, LOW);
+  }
+
   digitalWrite(bauSoftPin, bauSoftStatus);
 }