       }
     } 
     else {
-      if (millis()-blinkRedTimer>300){
-        stateRed=!stateRed;
-        blinkRedTimer=millis();
-        digitalWrite(LED_YELLOW, stateRed);   
+      stateGreen=false;
+      digitalWrite(LED_GREEN, stateGreen);
+      if (millis()-blinkYellowTimer>300){
+        stateYellow=!stateYellow;
+        blinkYellowTimer=millis();
+        digitalWrite(LED_YELLOW, stateYellow);   
       }
       Serial.print("*");
     }

char inStringBuffer[BUFFSIZE];
char outStringBuffer[BUFFSIZE];
uint32_t bytesRead = 0;
int blinkGreenTimer, blinkRedTimer;
bool stateRed,stateGreen;
bool emergency_mode,warning,FL_edge,FR_edge;
int emergency_sensor_threshold =880,currentState=IDLE;


          Serial.print("ERROR: Command "); Serial.print(command); Serial.print(", param1 "); Serial.print(param1); Serial.print(", param2 "); Serial.print(param2); Serial.println(", is not valid!");
        }
      }
      stateYellow=false;
      digitalWrite(LED_YELLOW, stateYellow);
      if (millis()-blinkGreenTimer>300){
        stateGreen=!stateGreen;
        blinkGreenTimer=millis();
