 USBHost Usb;
 ADK adk(&Usb, manufacturer, model, accessoryName, versionNumber, url, serialNumber);
 // End ADK configuration
+
 // Debug mode for communication
 bool COM_DEBUG_MODE = false;
+
 // Debug mode for movment
 bool MOV_DEBUG_MODE = false;
+
 uint8_t inBuffer[BUFFSIZE];
 uint8_t outBuffer[BUFFSIZE];
 char inStringBuffer[BUFFSIZE];
 char outStringBuffer[BUFFSIZE];
 uint32_t bytesRead = 0;
-int blinkGreenTimer, blinkYellowTimer;
-bool stateYellow,stateGreen;
-bool emergency_mode,warning,FL_edge,FR_edge;
-int emergency_sensor_threshold =800,currentState=IDLE;
-
-
-
-// Test Variabiles
-int randomint;
-int last_send = 0;
-
+
+bool 
+stateYellow,
+stateGreen,
+emergency_mode,
+warning,
+FL_edge,
+FR_edge;
 
 void setup() {
-  // Configure the A output
   pinMode(BRAKE_LEFT, OUTPUT);  // Brake pin on channel A
   pinMode(DIR_LEFT, OUTPUT);    // Direction pin on channel A
   pinMode(BRAKE_RIGHT, OUTPUT);  // Brake pin on channel B

bool sensorsOk(){  
  return !FLIsOut() && !FRIsOut();
}
bool FRIsOut(){
  return analogRead(FR)>emergency_sensor_threshold;
}
bool FLIsOut(){
  return analogRead(FL)>emergency_sensor_threshold;
}

void terminateAndroidApp(){
  sendToADK(INFO,TERMINATE,CMD_NULL_VALUE);  
}
    Serial.println(*param2);
  }
  return true;
}

void getDistance(){
  long duration, distance;
  digitalWrite(TRIG_PIN, LOW); 
